/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include"rosapi.h"
#include <nav_msgs/Odometry.h>
#include <std_msgs/UInt32.h>
#include <std_srvs/Empty.h>
#include <stdio.h>
#include <qdebug.h>
//#include "powermanager.h"
#include <QProcess>
#include <stdio.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <errno.h>
#include "domparser.h"
#include "lasercalculate.h"
#include "GeneralDefine.h"
#include "rf2o_laser_odometry/CLaserOdometry2D.h"
#include <fstream>
#include "yaml.h"
#include "messagelog.h"
#include "frames.hpp"
#include "reflectormatch.h"

#ifdef D_USE_VISION_MATCH
#include "visionmatch.h"
#endif

#ifdef D_USE_ICP
#include "pclicp.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#endif

//#include <sick_tim/sick_tim_common_usb.h>
//#include <sick_tim/sick_tim_common_tcp.h>
//#include <sick_tim/sick_tim_common_mockup.h>
//#include <sick_tim/sick_tim551_2050001_parser.h>
#include "amcl_node.h"
#include "amclconfig.h"
#include "laserdriversicktime.h"
#include "laserdriverkeli.h"
#include "laserdriverbeiyang.h"
#include "laserdriversicklms.h"
#include  <QFileInfo>

#define MSGKEY 10241024
//大地图加载导致激光接收超时
#define D_AMCL_NOCHECK_COUNT 60000/rosTimer

struct msgstru
{
   long msgtype;
   double x;
   double y;
   double rotate;
   double velocity;
};


//extern PowerManager *p_powerManger;

struct Vec3
{
   float x, y, a;
};

void operator >> (const YAML::Node& node, Vec3& v) {
   node[0] >> v.x;
   node[1] >> v.y;
   node[2] >> v.a;
}


#define D_SH_FILE_PATH "/agvwork/AgvFundation/fundation/bin/script/sh_file/"


RosApi::RosApi(QString defaultConfigFilePathIn):rosRate(25.0),
    move_base_client_("/move_base", true)
{
    robotId=-1;
    map_selector=0;
    isShutDown=false;
    isPclIcpWork=false;
    isVmarkWork=true;
    isReflectWork=true;
    isUseAmclAdvance=true;
    isUseSickTimDriver=true;
    isUseLaserOdometry=true;
    isUsePclIcp=false;
    defaultConfigFilePath=defaultConfigFilePathIn;
    visionPositionUpdateFailedCount=0;

//    frontLaserReceiveCount=0;
//    backLaserReceiveCount=0;

    pointCloudTrans_minConsumed=100000000000;
    pointCloudTrans_currentConsumed=0;
    pointCloudTrans_maxConsumed=0;

    rosThread_minConsumed=100000000000;
    rosThread_currentConsumed=0;
    rosThread_maxConsumed=0;

    odemeterFinalData_encoder.x=0;
    odemeterFinalData_encoder.y=0;
    odemeterFinalData_encoder.rotate=0;
    isEncoderOdomPublish=false;
    isMainProgramInitialOk=false;
    amclMaxDogCount=10;
    amclInitialPoseCalculateCount=10;
    isUseRosLaserMsg=true;
    laserAllowTimeOutCount=0;
    laserErrorTimeOutCount=0;
    laserDriverLoseDataCount[0]=0;
    laserDriverLoseDataCount[1]=0;

    vehiclePositionInfo_ros.isUpdateSucess=true;
    vehiclePositionInfo_ros.matchScore=10000;
    originalLaserFilterType=E_LASER_FILTER_TYPE_NONE;
    isInCreatingMap=false;

    debugFlag=0;

    vehiclePositionInfo_vmark.position.x =0;
    vehiclePositionInfo_vmark.position.y =0;
    vehiclePositionInfo_vmark.position.rotate=0;
    vehiclePositionInfo_vmark.matchScore=100;//可能没有初始化导致gsoap　type error
    vehiclePositionInfo_vmark.maxMatchError=0;//可能没有初始化导致gsoap　type error
    vehiclePositionInfo_vmark.isUpdateSucess=false;//可能没有初始化导致gsoap　type error

    vehiclePositionInfo_reflect.position.x =0;
    vehiclePositionInfo_reflect.position.y =0;
    vehiclePositionInfo_reflect.position.rotate=0;
    vehiclePositionInfo_reflect.matchScore=100;//可能没有初始化导致gsoap　type error
    vehiclePositionInfo_reflect.maxMatchError=0;//可能没有初始化导致gsoap　type error
    vehiclePositionInfo_reflect.isUpdateSucess=false;//可能没有初始化导致gsoap　type error

    isTfOk=true;
    isLaserLose=false;

    odomSolver=E_ODOM_SOLVER_ENCODER;
    frame_origin_encoderOdom=new KDL::Frame;
    (*frame_origin_encoderOdom)=frame_origin_encoderOdom->Identity();
    frame_compensation_encoderOdom=new KDL::Frame;
    (*frame_compensation_encoderOdom)=frame_compensation_encoderOdom->Identity();

    pclMatch=NULL;
    amclAdvance=NULL;

    laserScanDevice_infoList=new std::vector<LaserScanDeviceInfo>;
    threeDCameraDevice_infoList=new std::vector<LaserScanDeviceInfo> ;

    //设置ros日志消息级别
    if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) )
    {
       ros::console::notifyLoggerLevelsChanged();
    }

    if(1!=readConfigFile(defaultConfigFilePath))
    {
        qDebug()<<"error, RosApi readConfigFile";
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8157, robotId );
    }

    init();
    createThread();
    createThread2();
}

RosApi::~RosApi()
{

}

int RosApi::getReflectLaserFilterParameter(int index, LaserFilterParameter &paraOut)
{
    if(isUseReflectDetect && NULL!=reflectPositionMatch)
    {
        return reflectPositionMatch->getReflectLaserFilterParameter(index,paraOut);
    }
    return 0;
}

int RosApi::setReflectLaserFilterParameter(int index, LaserFilterParameter paraIn)
{
    if(isUseReflectDetect && NULL!=reflectPositionMatch)
    {
        return reflectPositionMatch->setReflectLaserFilterParameter(index,paraIn);
    }
    return 0;
}

int RosApi::getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut)
{
    if(isUseReflectDetect && NULL!=reflectPositionMatch)
    {
        return reflectPositionMatch->getReflectCalculateCommonParameter(paraOut);
    }
    return 0;
}

int RosApi::setReflectCalculateCommonParameter(ReflectCommonDetectPara paraIn)
{
    if(isUseReflectDetect && NULL!=reflectPositionMatch)
    {
        return reflectPositionMatch->setReflectCalculateCommonParameter(paraIn);
    }
    return 0;
}

int RosApi::getAmclDebugInfo(AmclDebugInfo &debugInfoOut)
{
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
        amclAdvance->getAmclDebugInfo(debugInfoOut);
    }
    #endif
    return 1;
}

int RosApi::setAmclPara(AmclDebugPara paraIn)
{
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
        amclAdvance->setAmclPara(paraIn);
    }
    #endif
    return 1;
}

int RosApi::getAmclPara(AmclDebugPara &paraOut)
{
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
        amclAdvance->getAmclPara(paraOut);
    }
    #endif
    return 1;
}

int RosApi::getLaserScanParameter(int laserId, LaserScanParameterHmi &infoOut)
{

    if(laserScanDevice_infoList->size()>laserId)
    {
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_x);
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_y);
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_z);
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_yaw);
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_pitch);
        infoOut.position.push_back((*laserScanDevice_infoList)[laserId].pose_roll);
        infoOut.minAngle=(*laserScanDevice_infoList)[laserId].laserMinAngle;
        infoOut.maxAngle=(*laserScanDevice_infoList)[laserId].laserMaxAngle;
        return 1;
    }
    return 0;
}

int RosApi::setLaserScanParameter(int laserId, LaserScanParameterHmi infoIn, int &returnFlag)
{
    //
    if(laserScanDevice_infoList->size()>laserId)
    {
        (*laserScanDevice_infoList)[laserId].pose_x=infoIn.position[0];
        (*laserScanDevice_infoList)[laserId].pose_y=infoIn.position[1];
        (*laserScanDevice_infoList)[laserId].pose_z=infoIn.position[2];
        (*laserScanDevice_infoList)[laserId].pose_yaw=infoIn.position[3];
        (*laserScanDevice_infoList)[laserId].pose_pitch=infoIn.position[4];
        (*laserScanDevice_infoList)[laserId].pose_roll=infoIn.position[5];
        (*laserScanDevice_infoList)[laserId].laserMinAngle=infoIn.minAngle;
        (*laserScanDevice_infoList)[laserId].laserMaxAngle=infoIn.maxAngle;
    }
    if(isUseReflectDetect)
    {
        reflectPositionMatch->setLaserLinkInBaseLink(infoIn.position[0],infoIn.position[1],infoIn.position[2],
                infoIn.position[5],infoIn.position[4],infoIn.position[3]);
    }
    if(isUseVmarkDetect)
    {
        visionPositionMatch->setLaserLinkInBaseLink(infoIn.position[0],infoIn.position[1],infoIn.position[2],
                infoIn.position[5],infoIn.position[4],infoIn.position[3]);
    }

    returnFlag=0;
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    tmpFilePath=tmpFilePath+D_LASER_SCAN_DEVICE_CONFIG_FILE_PATH;

    DomParser domparser;
    bool ok=domparser.openXml( tmpFilePath, QFile::ReadWrite | QFile::Text );
    if(!ok)
    {
        qDebug()<<"setLaserScanParameter error "<<ok;
        return -1;
    }

    QDomNodeList devNodes = domparser.getRootDomElement().childNodes();
    if(devNodes.size()<=laserId || 0>laserId)
    {
        qDebug()<<"setLaserScanParameter error laserId"<<laserId;
        return -2;
    }

    qDebug()<<"LaserScanParameterHmi infoIn.position"<<infoIn.position[0]<<infoIn.position[1]<<infoIn.position[2]
            <<infoIn.position[3]<<infoIn.position[4]<<infoIn.position[5];

        QDomNode devND =  devNodes.at(laserId);

        domparser.writeXml( domparser.findSubNode( devND, "pose_x", ok ), infoIn.position[0] );
        if(!ok)
        {
           return -100-4;
        }
        domparser.writeXml( domparser.findSubNode( devND, "pose_y", ok ), infoIn.position[1] );
        if(!ok)
        {
           return -100-5;
        }
        domparser.writeXml( domparser.findSubNode( devND, "pose_z", ok ), infoIn.position[2] );
        if(!ok)
        {
           return -100-6;
        }
        domparser.writeXml( domparser.findSubNode( devND, "pose_yaw", ok ), infoIn.position[3] );
        if(!ok)
        {
           return -100-7;
        }
        domparser.writeXml( domparser.findSubNode( devND, "pose_pitch", ok ), infoIn.position[4] );
        if(!ok)
        {
           return -100-8;
        }
        domparser.writeXml( domparser.findSubNode( devND, "pose_roll", ok ), infoIn.position[5] );
        if(!ok)
        {
           return -100-9;
        }

        domparser.writeXml( domparser.findSubNode( devND, "laserMinAngle", ok ), infoIn.minAngle );
        if(!ok)
        {
           return -100-10;
        }
        domparser.writeXml( domparser.findSubNode( devND, "laserMaxAngle", ok ), infoIn.maxAngle );
        if(!ok)
        {
           return -100-11;
        }



    domparser.closeXml(true);

    returnFlag=1;
    return 1;
}

int RosApi::getMapPropertyInfo(string mapName, MapPropertyInfo &infoOut)
{

    if(""==mapName)
    {
        if(""!=currentMapFileName)
        {
            QString   filePath = D_MAP_PATH;
            infoOut.mapName=currentMapFileName.toStdString();
            QString imageName;
            filePath+=currentMapFileName;
            readYamlFile(filePath,infoOut.mapRatio,
                         infoOut.xOffset,infoOut.yOffset,infoOut.rOffset,imageName);
            infoOut.imageName=imageName.toStdString();
            filePath = D_MAP_PATH;
            filePath+=imageName;
            getMapFileSize(filePath,infoOut.imageWidth,infoOut.imageHeight);
        }
        else
        {
            infoOut.mapName="";
            infoOut.imageName="";
            infoOut.mapRatio=0;
        }

    }
    return 1;
}

int RosApi::setVisionMatchKalmanFilterPara(int filterId, double followQ, double observeR)
{

    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setKalmanFilterPara(filterId,followQ,observeR);
    #endif

}

int RosApi::setReflectInitialVehiclePosition(VehiclePosition agvPoseIn)
{
    return reflectPositionMatch->setInitialVehiclePosition(agvPoseIn);
}

int RosApi::setReflectMatchDebugFlag(int flagIn)
{
    return reflectPositionMatch->setDebugFlag(flagIn);
}

int RosApi::getReflectPositionCertainity(VehiclePosition& uncertainPoseOut)
{
    return reflectPositionMatch->getReflectPositionCertainity(uncertainPoseOut);
}

int RosApi::getVmarkPositionCertainity(VehiclePosition &uncertainPoseOut)
{
    return visionPositionMatch->getVmarkPositionCertainity(uncertainPoseOut);
}

int RosApi::getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut)
{
    return reflectPositionMatch->getAllReferenceMarks(reflectMarksOut);
}

int RosApi::addReferenceMarkToMap(ReflectMarkInfo markIn)
{
    return reflectPositionMatch->addReferenceMarkToMap(markIn);
}

int RosApi::saveReferenceMarkToFile(std::string mapFileName_yaml)
{
    QString  mapName=QString::fromStdString(mapFileName_yaml);
    qDebug()<<"RosApi::saveReferenceMarkToFile "<<mapName;

    QRegExp rx("(.*).yaml");

    bool isMatch=rx.exactMatch(mapName);

    if(false==isMatch )
    {
        qDebug()<<"map file name error "<<mapName;
        return -1;
    }

    QString mapOriginalName=rx.cap(1);

    QString   filePath = D_MAP_PATH + mapOriginalName+".ref";
    return reflectPositionMatch->saveReferenceMarkToFile(filePath);
}

int RosApi::deleteReferenceMarkInMap(int uniqId)
{
    return reflectPositionMatch->deleteReferenceMarkInMap(uniqId);
}

int RosApi::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    return 1;
}

int RosApi::setAmclIsCalculateLaser(bool isCalculateLaserIn)
{
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
        amclAdvance->setAmclIsCalculateLaser(isCalculateLaserIn);
    }
    #endif
    return 1;
}

bool RosApi::isTfOkStatus()
{
    return isTfOk;
}

int RosApi::getLaserLoseStatus(QVector<bool> &isLaseLoseListOut)
{
    isLaseLoseListOut.clear();
    isLaseLoseListOut.append(isLaserLose);
    return 1;
}

int RosApi::getInternalLaserDriverStatus(QVector<int> &statusOut)
{
    statusOut.clear();
    if(false==isUseRosLaserMsg)
    {
        statusOut.append(internalLaserDriverStatus[0]);
        statusOut.append(internalLaserDriverStatus[1]);
    }

    return 1;
}

bool RosApi::isInCreatingMapStatus()
{
    return isInCreatingMap;

}

int RosApi::setLineFittingMethod(int methodIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setLineFittingMethod(methodIn);
    #endif
}

int RosApi::setIsPrintVmarkProcessData(bool isPrint)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setIsPrintVmarkProcessData(isPrint);
    #endif
}

int RosApi::setOriginalLaserFilterType(E_LASER_FILTER_TYPE typeIn)
{
    originalLaserFilterType=typeIn;
    return 1;
}

int RosApi::getCurrentOriginalLaserFilterType(E_LASER_FILTER_TYPE &typeOut)
{
    typeOut=originalLaserFilterType;
    return 1;
}

int RosApi::updateVehicleVelocity(VehicleVelocity velocityIn, timespec velocityUpdataStampIn)
{
    currentVehicleVelocity=velocityIn;
    velocityUpdataStamp=velocityUpdataStampIn;
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
        double composedVel=sqrt(currentVehicleVelocity.x_move*currentVehicleVelocity.x_move+
                                currentVehicleVelocity.y_move*currentVehicleVelocity.y_move);
        amclAdvance->setVehicleVelocityFeedback(composedVel,currentVehicleVelocity.z_rotate);
    }
    #endif
    return 1;
}

int RosApi::getVMarkPositionInWorld(VehiclePosition &positionOut, bool &isSucess)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->getVMarkPositionInWorld(positionOut,isSucess);
    #endif
}



int RosApi::setLaserIntensityFilterByType_visionMatch(int typeIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setLaserIntensityFilterByType(typeIn);
    #endif
}

int RosApi::getMatchDebugInfo_vmark(VisionMatchDebugInfo &infoOut)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->getMatchDebugInfo(infoOut);
    #endif
}

int RosApi::getMatchDebugInfo_reflect(ReflectDebugInfo &infoOut)
{
    return reflectPositionMatch->getDebugInfo(infoOut);
}

int RosApi::calculateVMarkPosition_byLaser(double baseLink_x, double baseLink_y, double baseLink_r, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    #ifdef D_USE_VISION_MATCH
//    return visionPositionMatch->calculateVMarkPosition_byLaser(baseLink_x,baseLink_y,baseLink_r,x_out,y_out,r_out);
    #endif
}

int RosApi::updateVMarkFile(QString vmarkFileIn)
{
    #ifdef D_USE_VISION_MATCH
    QString mapFilePath=D_MAP_PATH;
    vmarkFileIn=mapFilePath+vmarkFileIn;
    return visionPositionMatch->updateVMarkFile(vmarkFileIn);
    #endif
}

int RosApi::createVMark(VMarkInfo infoIn, QString vmarkFileIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->createVMark(infoIn,vmarkFileIn);
    #endif
}

int RosApi::getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->getVmarkInfoList(vmarkInfoOut);
    #endif
}

int RosApi::setCurrentVMarkIndex(int indexIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setCurrentVMarkIndex(indexIn);
    #endif
}

int RosApi::setCurrentVMarkType(int currentVmarkTypeIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setCurrentVMarkType(currentVmarkTypeIn);
    #endif
}

int RosApi::detectVMark_baseMap(vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    #ifdef D_USE_VISION_MATCH
     return visionPositionMatch->detectVMark_baseMap(x_out,y_out,r_out);
    #endif
}

int RosApi::detectVMark_laser(int vmark_index,vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    #ifdef D_USE_VISION_MATCH
//     return visionPositionMatch->detectVMark_laser(vmark_index,x_out,y_out,r_out);
    #endif
}

int RosApi::showDebugWindow_visionMatch(int id)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->showDebugWindow(id);
    #endif
}

int RosApi::setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn)
{
    #ifdef D_USE_VISION_MATCH
    return visionPositionMatch->setVmarkDetectParameter(vmarkParameterIn);
    #endif

}

int RosApi::getTimeConsumed(int componentId, double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    switch(componentId)
    {
    case 1:
    {
        #ifdef D_USE_AMCL_ADVANCE
        if(NULL==amclAdvance)
        {
            return -1;
        }
        amclAdvance->getTimeConsumed(minConsumedOut,currentConsumedOut,maxConsumedOut);
        #endif
        break;
    }
    case 2:
    {
        if(NULL==laserOdometry)
        {
            return -1;
        }
        #ifdef D_USE_LASER_ODOMETRY
        laserOdometry->getTimeConsumed(minConsumedOut,currentConsumedOut,maxConsumedOut);
        #endif
        break;
    }
    case 3:
    {
        if(NULL==visionPositionMatch)
        {
            return -1;
        }
        #ifdef D_USE_VISION_MATCH
        visionPositionMatch->getTimeConsumed(minConsumedOut,currentConsumedOut,maxConsumedOut);
        #endif
        break;
    }
    case 4://点云转换
    {
        minConsumedOut=pointCloudTrans_minConsumed;
        currentConsumedOut=pointCloudTrans_currentConsumed;
        maxConsumedOut=pointCloudTrans_maxConsumed;
        break;
    }
    case 5://rosThread
    {
        minConsumedOut=rosThread_minConsumed;
        currentConsumedOut=rosThread_currentConsumed;
        maxConsumedOut=rosThread_maxConsumed;
        break;
    }
    }

    return 1;

}

int RosApi::resetRosMeasureTime()
{
    #ifdef D_USE_AMCL_ADVANCE
    if(NULL!=amclAdvance)
    {
            amclAdvance->resetRosMeasureTime();
    }
    #endif

    #ifdef D_USE_LASER_ODOMETRY
    if(NULL!=laserOdometry)
    {

            laserOdometry->resetRosMeasureTime();
    }
    #endif

    #ifdef D_USE_VISION_MATCH
    if(NULL!=visionPositionMatch)
    {
        visionPositionMatch->resetRosMeasureTime();
    }
    #endif

    pointCloudTrans_minConsumed=100000;
    pointCloudTrans_maxConsumed=-100000;

    rosThread_minConsumed=100000;
    rosThread_maxConsumed=-100000;

    return 1;
}

int RosApi::getAllPointCloud_translated(std::vector<PointCloudData_3D> &pointCloudOut)
{
    pthread_mutex_lock(&rosDataMutex);
    pointCloudOut.clear();
    pointCloudOut.push_back(*frontOriginLaserData_pointCloud_translated);
//    pointCloudOut.push_back(*frontFilterLaserData_pointCloud_translated);
    pointCloudOut.push_back(*backOriginLaserData_pointCloud_translated);
    pointCloudOut.push_back(*front_3dCameraData_translated);
    pointCloudOut.push_back(*back_3dCameraData_translated);
    pthread_mutex_unlock(&rosDataMutex);
    return 1;
}

int RosApi::getAllPointCloud_translated_qvector(QVector<PointCloudData_3D> &pointCloudOut)
{
    pthread_mutex_lock(&rosDataMutex);
    pointCloudOut.clear();
    pointCloudOut.append(*frontOriginLaserData_pointCloud_translated);
    pointCloudOut.append(*backOriginLaserData_pointCloud_translated);
    pointCloudOut.append(*front_3dCameraData_translated);
    pointCloudOut.append(*back_3dCameraData_translated);
    pthread_mutex_unlock(&rosDataMutex);
}

int RosApi::getAllPointCloud_translated(PointCloudData_3D &pointCloudOut)
{
    pthread_mutex_lock(&rosDataMutex);

    pointCloudOut.deviceName.clear();
    pointCloudOut.point_x.clear();
    pointCloudOut.point_y.clear();
    pointCloudOut.point_z.clear();
    insertPointCloud(pointCloudOut,*frontOriginLaserData_pointCloud_translated);
    insertPointCloud(pointCloudOut,*backOriginLaserData_pointCloud_translated);
    insertPointCloud(pointCloudOut,*front_3dCameraData_translated);
    insertPointCloud(pointCloudOut,*back_3dCameraData_translated);

    pthread_mutex_unlock(&rosDataMutex);
    return 1;

}

int RosApi::getPointCloud(string deviceName, PointCloudData_3D &pointCloudOut)
{
    if("front_scan"==deviceName)
    {
        pthread_mutex_lock(&rosDataMutex);
        pointCloudOut=*frontOriginLaserData_pointCloud;
        pthread_mutex_unlock(&rosDataMutex);
        return 1;
    }
    else if("back_scan"==deviceName)
    {
        pthread_mutex_lock(&rosDataMutex);
        pointCloudOut=*backOriginLaserData_pointCloud;
        pthread_mutex_unlock(&rosDataMutex);
        return 1;
    }
    else if("front_3dCamera"==deviceName)
    {
        pthread_mutex_lock(&rosDataMutex);
        pointCloudOut=*front_3dCameraData;
        pthread_mutex_unlock(&rosDataMutex);
        return 1;
    }
    else if("back_3dCamera"==deviceName)
    {
        pthread_mutex_lock(&rosDataMutex);
        pointCloudOut=*back_3dCameraData;
        pthread_mutex_unlock(&rosDataMutex);
        return 1;
    }
    else if("front_filter_laser"==deviceName)
    {
        pthread_mutex_lock(&rosDataMutex);
        pointCloudOut=*frontFilterLaserData_pointCloud;
        pthread_mutex_unlock(&rosDataMutex);
        return 1;
    }



    return -1;
}

int RosApi::setVehiclePoseEstimation(double x, double y, double r)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
     return pclMatch->setVehiclePoseEstimation(x,y,r);
    #endif
}

int RosApi::forceAmclUpdatePartial()
{
    std_srvs::Empty srvsPara;
    if(amcl_service->call(srvsPara))
    {
        qDebug()<<"forceAmclUpdatePartial sucess!";
        return 1;
    }
    qDebug()<<"forceAmclUpdatePartial failed!";
    return -1;
}

int RosApi::setPclIcpWork(bool isWork)
{
    isPclIcpWork=isWork;
    return 1;

}

int RosApi::setVmarkWork(bool isWork)
{
    visionPositionUpdateFailedCount=0;
    isVmarkWork=isWork;
    return 1;

}

int RosApi::setReflectWork(bool isWork)
{
    isReflectWork=isWork;
    return 1;
}

int RosApi::updatePclIcpDebugWindow()
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    return pclMatch->updatePclIcpDebugWindow();
    #endif
}

int RosApi::setMatchCalculateParameter_pcl(PclCalculationParameter paraIn)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    return pclMatch->setMatchCalculateParameter(paraIn);
    #endif
}

int RosApi::getMatchDebugInfo_pcl(PclMatchDebugInfo &infoOut)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    return pclMatch->getMatchDebugInfo(infoOut);
    #endif
}

int RosApi::showVisionMatchDebugWindow_pcl(bool isShowBaseMap, bool isShowBaseCompareMap,
                                       bool isShowLocalMap, bool isShowLocalCompareMap)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    return pclMatch->showPclIcpDebugWindow(isShowBaseMap,isShowBaseCompareMap,isShowLocalMap,isShowLocalCompareMap);
    #endif
}

int RosApi::setBaseMapRegion_pcl(MapRegion baseRegionIn)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    baseMapRegion=baseRegionIn;
    pclMatch->setBaseMapRegion_m(baseRegionIn.originX,baseRegionIn.originY,baseRegionIn.width,baseRegionIn.height);
    return 1;
    #endif
}

int RosApi::setLocalMapRegion_pcl(MapRegion localRegionIn)
{
    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
    localMapRegion=localRegionIn;
    pclMatch->setLocalMapRegion_m(localRegionIn.originX,localRegionIn.originY,localRegionIn.width,localRegionIn.height);
    return 1;
    #endif
}

int RosApi::setBaseMapRegion_visionMatch(MapRegion baseRegionIn)
{
    #ifdef D_USE_VISION_MATCH
    baseMapRegion=baseRegionIn;
//    visionPositionMatch->setBaseMapRegion_m(baseRegionIn.originX,baseRegionIn.originY,baseRegionIn.width,baseRegionIn.height);
    return 1;
    #endif
}

int RosApi::setLocalMapRegion_visionMatch(MapRegion localRegionIn)
{
    #ifdef D_USE_VISION_MATCH
    localMapRegion=localRegionIn;
//    visionPositionMatch->setLocalMapRegion_m(localRegionIn.originX,localRegionIn.originY,localRegionIn.width,localRegionIn.height);
    return 1;
    #endif

}

int RosApi::setLocalMapLaserRegion_visionMatch(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn)
{
    #ifdef D_USE_VISION_MATCH
    visionPositionMatch->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
    #endif
    return 1;
}

int RosApi::setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn)
{
    laserCalculate->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
    #ifdef D_USE_VISION_MATCH
    visionPositionMatch->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
    #endif
    reflectPositionMatch->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);

    return 1;

}

int RosApi::shutDownRosModule()
{
    qDebug()<<"shutDownRosModule";
    #ifdef D_USE_AMCL_ADVANCE
    if(true==isUseAmclAdvance)
    {
        if(isnan(vehiclePositionInfo_ros.position.x) || isnan(vehiclePositionInfo_ros.position.y)
                 ||isnan(vehiclePositionInfo_ros.position.rotate))
        {
             addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8172, robotId );
        }
        else
        {

//            amclConfig->writeConfigFile(vehiclePositionInfo_ros.position.x,vehiclePositionInfo_ros.position.y,
//                                        vehiclePositionInfo_ros.position.rotate);
        }
    }
    #endif

    isShutDown=true;
    ros::shutdown();
//    while(false==isRosLoopFinished)
//    {
//        sleep(1);
//    };关机时导致一直停在这里
//    shutdownSickTimDriver();
    #ifdef D_USE_INTERNAL_LASER_DRIVER
    for(int i=0;i<internalLaserScanDriver.size();i++)
    {
        internalLaserScanDriver[i]->shutDownDriver();
    }
    return 1;
    #endif

}

int RosApi::rosLaunchCommand(QString commandStr)
{
    qDebug()<<"launch_command"<<commandStr;
    int returnResult;
    const char* command_char;
    std::string systemCommand=commandStr.toStdString();
    systemCommand+=" &";
    command_char=systemCommand.c_str();
    int systemResult=system(command_char);
    if(systemResult==-1)
    {
        printf("launchCommand error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(systemResult))
        {
            if(0==WEXITSTATUS(systemResult))
            {
                printf("launchCommand successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("launchCommand failed %d \n",WEXITSTATUS(systemResult));
                returnResult=0;
            }
        }
        else
        {
            printf("launchCommand exit code %d \n",WEXITSTATUS(systemResult));
            returnResult=0;
        }
    }
    return returnResult;
}


int RosApi::getFrontLaserData(LaserDataInfo &frontLaserDataOut)
{
    pthread_mutex_lock(&rosDataMutex);
    frontLaserDataOut=*frontOriginLaserData;
    pthread_mutex_unlock(&rosDataMutex);
    return 1;
}

int RosApi::getAmclMatchScore(double &mapDistance_out,float &weight_out, std::vector<float> &pos_cov_out)
{
    if(false==isUseAmclAdvance)
    {
        return 0;
    }
    #ifdef D_USE_AMCL_ADVANCE
//    weight_out=amcl_match_score;
//    pos_cov_out=amcl_pos_cov;
    amclAdvance->getPositionCertaintyFactor(mapDistance_out,weight_out,pos_cov_out);
    #endif
    return 1;
}

int RosApi::setDefaultMapName(std::string fileNameIn,int &returnFlag)
{
    qDebug()<<"RosApi::setDefaultMapName"<<QString::fromStdString(fileNameIn);
    if(fileNameIn.length()<=0)
    {
        returnFlag=-1;
        return -1;
    }

    DomParser xmlParser;
    bool ok = xmlParser.openXml(defaultConfigFilePath, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, setDefaultMapName open file error！";
       returnFlag=-1;
       return -1;
    }

    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "CommSetting", ok );
    if( !ok )
    {

        qDebug()<<"error, setDefaultMapName node error！";
        returnFlag=-1;
        return -1;
    }

    //节点写数据
    bool keyReturn;
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "defaultMapFileName", keyReturn),
                        QString::fromStdString(fileNameIn) );
    if( !keyReturn )
    {

        qDebug()<<"error, setDefaultMapName node error！";
        returnFlag=-1;
        return -1;
    }
    xmlParser.closeXml(true);

    returnFlag=1;
    return 1;

}

int RosApi::getCurrentMapName(string &mapNameOut)
{
    pthread_mutex_lock(&rosDataMutex);
    mapNameOut=currentMapFileName.toStdString();
    pthread_mutex_unlock(&rosDataMutex);
    return 1;

}

int RosApi::setMapResolution(double cmPerPixIn)
{
    //need todo hualei. 需要同步到ｇmapping配置文件，对建立地图有效

    ///home/tcr/tcr100_ws/src/tcr100/launch/mapping/gmapping_with_one_lidar.launch
    /// <param name="delta" value="0.025"/>

}


bool RosApi::init()
{

    #ifdef D_USE_ICP
    if(true==isUsePclIcp)
    {
         pclMatch=new PclIcp;
    }
    #endif

    #ifdef D_USE_VISION_MATCH
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString infoFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number(1)+"/config/visionMatch.xml";
    visionPositionMatch=new VisionMatch(infoFile,1,vmarkCalculateSkip);
    #endif
    tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    infoFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number(1)+"/config/reflectMatch.xml";
    reflectPositionMatch=new ReflectorMatch(infoFile,1,reflectCalculateSkip);//不能放在ros_init()里面，可能文件操作对ｒｏｓ初始化有影响。

    ros_init();
//    initial_message();

    //初始化线程
    currentThread=new QThread();
    currentThread->start();
    while( !currentThread->isRunning() )
    {
        usleep(100);
    }
    qDebug()<< " ros qt thread created! ";
    this->moveToThread(currentThread);

    //连接信号槽
    connect(this, SIGNAL(startNavigation_signal()), this, SLOT(startNavigation_slot()) );
    connect(this, SIGNAL(moveToPosition_signal(VehiclePosition,double)), this,
            SLOT(moveToPosition_slot(VehiclePosition,double)));
//    timer.start(500);
//    connect(&timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()));
    initalLaserDevice(*laserScanDevice_infoList);
    inital3dCameraDevice(*threeDCameraDevice_infoList);
    loadDefaultNavagationParameter(defaultConfigFilePath);
    for(int i=0;i<ros_launch_command.size();i++)
    {
        rosLaunchCommand(ros_launch_command[i]);
    }

    if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) )
    {
       ros::console::notifyLoggerLevelsChanged();
    }


}

bool RosApi::ros_init()
{
    //ros 对象initial
//    nh_=new ros::NodeHandle;
//    rosRate=new ros::Rate;
    current_goal=new move_base_msgs::MoveBaseGoal;
//    move_base_client_=new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>() ;
    initial_position_pub_=new ros::Publisher;
    goal_pub_=new ros::Publisher;
    front_filter_laser_pub_=new ros::Publisher;
    amcl_match_score_sub_=new ros::Subscriber;
    move_base_status_sub_=new ros::Subscriber;
    move_base_path_sub_=new ros::Subscriber;
    move_base_goal_sub_=new ros::Subscriber;
    cmd_vel_=new ros::Subscriber ;
    pose_sub_=new ros::Subscriber ;
    front_laser_data_sub_=new ros::Subscriber;
    back_laser_data_sub_=new ros::Subscriber ;
    front_3dCamera_data_sub_=new ros::Subscriber ;//todo
    back_3dCamera_data_sub_=new ros::Subscriber ;//todo
    odom_pub_=new ros::Publisher ;
    creatMap_pub_=new ros::Publisher ;
    amcl_service=new ros::ServiceClient ;
    listener=new tf::TransformListener ;
    tfBroadcaster=new tf::TransformBroadcaster;

    //---------初始化锁--------
    pthread_mutexattr_init(&rosDataAttr);
    pthread_mutexattr_setprotocol(&rosDataAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&rosDataMutex,&rosDataAttr);
    frontOriginLaserData =new LaserDataInfo;
    frontFilterLaserData =new LaserDataInfo;
    backOriginLaserData=new LaserDataInfo;
    frontOriginLaserData_pointCloud=new PointCloudData_3D;
    frontOriginLaserData_pointCloud->deviceName="front_scan";
    frontOriginLaserData_pointCloud->timeStamp=0;
    frontFilterLaserData_pointCloud=new PointCloudData_3D;
    frontFilterLaserData_pointCloud->deviceName="front_filter_laser";
    frontFilterLaserData_pointCloud->timeStamp=0;
    backOriginLaserData_pointCloud=new PointCloudData_3D;
    backOriginLaserData_pointCloud->deviceName="back_scan";
    backOriginLaserData_pointCloud->timeStamp=0;
    front_3dCameraData=new PointCloudData_3D;
    front_3dCameraData->deviceName="front_3dCamera";
    front_3dCameraData->timeStamp=0;
    back_3dCameraData=new PointCloudData_3D;
    back_3dCameraData->deviceName="back_3dCamera";
    back_3dCameraData->timeStamp=0;

    frontOriginLaserData_pointCloud_translated=new PointCloudData_3D;
    frontOriginLaserData_pointCloud_translated->deviceName="front_scan_translated";
    frontOriginLaserData_pointCloud_translated->timeStamp=0;
    frontFilterLaserData_pointCloud_translated=new PointCloudData_3D;
    frontFilterLaserData_pointCloud_translated->deviceName="front_filter_laser_translated";
    frontFilterLaserData_pointCloud_translated->timeStamp=0;
    backOriginLaserData_pointCloud_translated=new PointCloudData_3D;
    backOriginLaserData_pointCloud_translated->deviceName="back_scan_translated";
    backOriginLaserData_pointCloud_translated->timeStamp=0;
    front_3dCameraData_translated=new PointCloudData_3D;
    front_3dCameraData_translated->deviceName="front_3dCamera_translated";
    front_3dCameraData_translated->timeStamp=0;
    back_3dCameraData_translated=new PointCloudData_3D;
    back_3dCameraData_translated->deviceName="back_3dCamera_translated";
    back_3dCameraData_translated->timeStamp=0;


    laserCalculate= new LaserCalculate;


    *initial_position_pub_= nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10, true);
//    *amcl_match_score_sub_=nh_.subscribe("/mapMatchScore", 10, &RosApi::amcl_match_scoreCallback, this);
    *move_base_status_sub_ = nh_.subscribe("/move_base/status", 10, &RosApi::moveBaseResultCallback, this);
    *odom_pub_=nh_.advertise<nav_msgs::Odometry>("/odom_encoder", 10, true);
    *creatMap_pub_=nh_.advertise<std_msgs::UInt32>("/create_map", 10, true);
    if(true==isUseRosLaserMsg)
    {
        *front_laser_data_sub_ = nh_.subscribe("/front_scan", 10, &RosApi::frontLaserScanDataCallback, this);
        *back_laser_data_sub_ = nh_.subscribe("/back_scan", 10, &RosApi::backLaserScanDataCallback, this);
    }

//    *front_3dCamera_data_sub_  = nh_.subscribe("/front_3dCamera", 5, &RosApi::front3dCameraDataCallback, this);
    *front_3dCamera_data_sub_  = nh_.subscribe("/camera/depth_registered/points", 5, &RosApi::front3dCameraDataCallback, this);
    *back_3dCamera_data_sub_  = nh_.subscribe("/back_3dCamera", 10, &RosApi::back3dCameraDataCallback, this);

    *cmd_vel_ = nh_.subscribe("/cmd_vel", 10, &RosApi::twistCmdVelCallback, this);
    *move_base_path_sub_=nh_.subscribe("/move_base/NavfnROS/plan", 10, &RosApi::gloablPathLenghtCallback, this);
    *move_base_goal_sub_=nh_.subscribe("/move_base/goal", 10, &RosApi::movebaseGoalCallback, this);
    *pose_sub_=nh_.subscribe("/amcl_pose", 10, &RosApi::robotPoseCallback, this);
    *goal_pub_=nh_.advertise<tcr100::VehicleGoal>("/vehicle_goal", 10, true);
    *front_filter_laser_pub_=nh_.advertise<sensor_msgs::LaserScan>("/front_filter_laser_scan", 5, true);

    *amcl_service=nh_.serviceClient<std_srvs::Empty>("request_nomotion_update");

//    initialSickTimDriver();
    initialAmclAdvance();
    initialLaserOdometry();



    return true;
}

bool RosApi::initial_message()
{
    msqid=msgget(MSGKEY,IPC_EXCL);  /*检查消息队列是否存在*/
    if(msqid < 0)
    {
      msqid = msgget(MSGKEY,IPC_CREAT|0666);/*创建消息队列*/
      if(msqid <0)
      {
      printf("failed to create msq | errno=%d [%s]\n",errno,strerror(errno));
//      exit(-1);
      }
    }
}

int RosApi::sendPositionMessage(VehiclePosition positionIn)
{
    struct msgstru msgs;
    int msg_type;
    char str[256];
    int ret_value;
    msgs.msgtype = 999;
    msgs.x=positionIn.x;
    msgs.y=positionIn.y;
    msgs.rotate=positionIn.rotate;
    /* 发送消息队列 */
    ret_value = msgsnd(msqid,&msgs,sizeof(struct msgstru),IPC_NOWAIT);
    if ( ret_value < 0 )
    {
       printf("msgsnd() write msg failed,errno=%d[%s]\n",errno,strerror(errno));
//       exit(-1);
       return -1;
    }
    return 1;
}



//建图选择
int RosApi::setMapSolver(struct UserInfo userInfo,enum E_MAP_SOLVER solverIn,int laserCount,int &returnFlag)
{
    mapSolver=solverIn;
    int returnResult;
    if(laserCount==1)
    {
        switch (solverIn) {

        case E_MAP_SOLVER_GMAPPING:
            map_selector=0;
            returnResult=1;
            break;
        case E_MAP_SOLVER_CARTOGRAPHER:
            map_selector=1;
            returnResult=1;
            break;
        case E_MAP_SOLVER_HECTOR:
            map_selector=2;
            returnResult=1;
            break;
        default:
            map_selector=0;
            returnResult=0;
            break;
        }
    }
    else if(laserCount==2)
    {
        switch (solverIn) {

        case E_MAP_SOLVER_GMAPPING:
            map_selector=3;
            returnResult=1;
            break;
        case E_MAP_SOLVER_CARTOGRAPHER:
            map_selector=4;
            returnResult=1;
            break;
        case E_MAP_SOLVER_HECTOR:
            map_selector=5;
            returnResult=1;
            break;
        default:
            map_selector=3;
            returnResult=0;
            break;
        }
    }
    return returnResult;
}

//选择odom
int RosApi::setOdomSolver(struct UserInfo userInfo,enum E_ODOM_SOLVER solverIn,int &returnFlag)
{
    //编码器和激光里程计切换逻辑有问题，位置变化较大。无论是激光里程计一起运行还是切换后再运行，
    //激光里程计跑告诉对于ｃｐｕ和激光数据的密度要求可能比较高。频繁切换时，出现１次coredump。
    if(E_ODOM_SOLVER_ENCODER==odomSolver && E_ODOM_SOLVER_LASER==solverIn)
    {
       isEncoderOdomPublish=false;
//       laserOdometry->syncOdom(odemeterFinalData_encoder.x,odemeterFinalData_encoder.y,
//                               odemeterFinalData_encoder.rotate);

       PositionInfo tmp_vehiclePositionInfo_ros=vehiclePositionInfo_ros;
       #ifdef D_USE_LASER_ODOMETRY
       laserOdometry->setPublishEnable(true);
       laserOdometry->odometryCalculation();//强制先更新激光里程计
       #endif
       usleep(500000);
       int tmpReturn;
       setVehiclePosition(userInfo,tmp_vehiclePositionInfo_ros.position,D_SET_ODOMETER_COVE_RATIO,tmpReturn);//重新初始化位置
       usleep(1000000);

    }
    else if(E_ODOM_SOLVER_LASER==odomSolver && E_ODOM_SOLVER_LASER==solverIn)
    {
        #ifdef D_USE_LASER_ODOMETRY
        laserOdometry->setPublishEnable(true);
        #endif
    }

    if(E_ODOM_SOLVER_LASER==odomSolver && E_ODOM_SOLVER_ENCODER==solverIn)
    {
        #ifdef D_USE_LASER_ODOMETRY
        laserOdometry->setPublishEnable(false);
        #endif
//        VehiclePosition startOdome;
//        laserOdometry->getFinalOdom(startOdome.x,startOdome.y,startOdome.rotate);
//        syncEncoderOdom(startOdome);

        PositionInfo tmp_vehiclePositionInfo_ros=vehiclePositionInfo_ros;
        int tmpReturn;
        isEncoderOdomPublish=true;
        usleep(500000);
        setVehiclePosition(userInfo,tmp_vehiclePositionInfo_ros.position,D_SET_ODOMETER_COVE_RATIO,tmpReturn);
        usleep(1000000);

    }
    else if(E_ODOM_SOLVER_ENCODER==odomSolver && E_ODOM_SOLVER_ENCODER==solverIn)
    {
        isEncoderOdomPublish=true;
    }

    odomSolver=solverIn;
    int returnResult;
    std::string odom_algorithm;
    switch (solverIn) {
    case E_ODOM_SOLVER_ENCODER:
        //base_link_to_front_laser is a required node
//        system("rosnode kill /base_link_to_front_laser &");
//        odom_algorithm="roslaunch tcr100 odom_from_encoder.launch &";

        break;

    case E_ODOM_SOLVER_LASER:
//        system("rosnode kill /base_link_to_front_laser &");
//        odom_algorithm="roslaunch tcr100 odom_from_laser.launch &";
        break;
    case E_ODOM_SOLVER_ENCODER_IMU:
//        system("rosnode kill /base_link_to_front_laser &");
//        odom_algorithm="roslaunch tcr100 odom_from_encoder_and_imu.launch &";
        break;
    case E_ODOM_SOLVER_LASER_ENCODER_IMU:
//        system("rosnode kill /base_link_to_front_laser &");
//        odom_algorithm="roslaunch tcr100 odom_from_encoder_laser_and_imu.launch &";
        break;
    default:
        //system("rosnode kill /base_link_to_front_laser &");
//        odom_algorithm="roslaunch tcr100 odom_from_encoder.launch &";
//        returnResult=0;
        break;
    }

    return 1;
}

//开始建图
int RosApi::createMapStart(struct UserInfo userInfo,int &returnResult)
{
    if(isInCreatingMap)
    {
        qDebug()<<"InCreatingMap!!!";
        addMsg(ENUM_MSG_REMIND, "RosApi", "RosApi", 8179, robotId );
        returnResult=1;
        return 1;
    }
    pthread_mutex_lock(&rosDataMutex);
    currentMapFileName="";
    pthread_mutex_unlock(&rosDataMutex);

    UserInfo tmpUser;
    //关闭movebase节点
//    stopNavigation(tmpUser,returnResult);会导致sick lms驱动崩溃重启，地图创建服务不启动（或不成功）。
    #ifdef D_USE_AMCL_ADVANCE
    if(true==isUseAmclAdvance)
    {
        amclAdvance->setWorkStatus(false);
    }
    #endif

    std_msgs::UInt32 createInfo;
    if(map_selector==0)
    {
            createInfo.data=1;
    }
    else if(map_selector==1)
    {
            createInfo.data=2;
    }
    else
    {
        qDebug()<<"ERROR ,NOT SUPPORT CREAT MAP METHOD!!!";
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8163, robotId );
        returnResult=-2;
        return -1;
    }

    creatMap_pub_->publish(createInfo);//通过ros消息的方式通知地图节点开始创建。
    returnResult=1;

    isInCreatingMap=true;
    qDebug()<<"RosApi::createMapStart";
    addMsg(ENUM_MSG_REMIND, "RosApi", "RosApi", 8163, robotId );

//    std::string mapping_algorithm;
//    if(map_selector==0)
//    {
//        mapping_algorithm="roslaunch tcr100 gmapping_with_one_lidar.launch &";
//    }
//    else if(map_selector==1)
//    {
//        mapping_algorithm="roslaunch tcr100 cartographer_with_one_lidar.launch &";
//    }
//    else if(map_selector==2)
//    {
//        mapping_algorithm="roslaunch tcr100 hector_with_one_lidar.launch &";
//    }
//    else if(map_selector==3)
//    {
//        mapping_algorithm="roslaunch tcr100 gmapping_with_two_lidars.launch &";
//    }
//    else if(map_selector==4)
//    {
//        mapping_algorithm="roslaunch tcr100 cartographer_with_two_lidars.launch &";
//    }
//    else if(map_selector==5)
//    {
//        mapping_algorithm="roslaunch tcr100 hector_with_two_lidars.launch &";
//    }
//    else
//    {
//        returnResult=0;
//    }
//    const char* mapping_algorithm_sh=mapping_algorithm.c_str();
//    int mapping_algorithm_status=system(mapping_algorithm_sh);
//    if(mapping_algorithm_status==-1)
//    {
//        printf("mapping_algorithm system error... \n");
//        returnResult=0;
//    }
//    else
//    {

//        if(WIFEXITED(mapping_algorithm_status))
//        {
//            if(0==WEXITSTATUS(mapping_algorithm_status))
//            {
//                printf("mapping_algorithm run successfully... \n");
//                returnResult=1;
//            }
//            else
//            {
//                printf("mapping_algorithm run failed %d \n",WEXITSTATUS(mapping_algorithm_status));
//                returnResult=0;
//            }
//        }
//        else
//        {
//            printf("mapping_algorithm exit code %d \n",WEXITSTATUS(mapping_algorithm_status));
//            returnResult=0;
//        }
//    }
    return returnResult;
}

//结束建图
int RosApi::createMapEnd(struct UserInfo userInfo, std::string mapName, int &returnResult)
{

    if(false==isInCreatingMap)
    {
        returnResult=-200;
        return returnResult;
    }
    if(mapName.length()<=0)
    {
        returnResult=-2;
        return returnResult;
    }



    std::string mapFile_path=D_SH_FILE_PATH;
    std::string map_path;

    //删除旧文件
    QString oldGmp=QString(D_MAP_PATH)+QString::fromStdString(mapName)+QString(".pgm");
    QString oldYaml=QString(D_MAP_PATH)+QString::fromStdString(mapName)+QString(".yaml");
    QFile::remove(oldGmp);
    QFile::remove(oldYaml);

    if(map_selector==0)
    {
        map_path="map_saver_gmapping_one.sh ";
    }
    else if(map_selector==1)
    {
        map_path="map_saver_cartographer_one.sh ";
    }
    else if(map_selector==2)
    {
        map_path="map_saver_hector_one.sh ";
    }
    else if(map_selector==3)
    {
        map_path="map_saver_gmapping_two.sh ";
    }
    else if(map_selector==4)
    {
        map_path="map_saver_cartographer_two.sh ";
    }
    else if(map_selector==5)
    {
        map_path="map_saver_hector_two.sh ";
    }
    else
    {
        returnResult=-3;
    }
    mapFile_path+=map_path;
    mapFile_path+=mapName;
    mapFile_path+="&";

    const char* map_saver_name=mapFile_path.c_str();
    printf("map_saver cmd: %s \n",map_saver_name);
    int map_saver_status=system(map_saver_name);
    if(map_saver_status==-1)
    {
        printf("map_saver system error... \n");
        returnResult=-4;
    }
    else
    {
        if(WIFEXITED(map_saver_status))
        {
            if(0==WEXITSTATUS(map_saver_status))
            {
                printf("map_saver run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("map_saver run failed %d \n",WEXITSTATUS(map_saver_status));
                returnResult=-5;
            }
        }
        else
        {
            printf("map_saver exit code %d \n",WEXITSTATUS(map_saver_status));
            returnResult=0;
        }
    }

    //初始化导航,地图保存还没有执行完毕，就执行下面的语句会导致地图保存错误
//    setDefaultMapName(QString::fromStdString(mapName));
//    loadDefaultNavagationParameter(defaultConfigFilePath);
#ifdef D_USE_AMCL_ADVANCE
    if(true==isUseAmclAdvance)
    {
          amclAdvance->setWorkStatus(true);
    }
#endif
    pthread_mutex_lock(&rosDataMutex);
    currentMapFileName.clear();
    pthread_mutex_unlock(&rosDataMutex);
    isInCreatingMap=false;
    qDebug()<<"RosApi::createMapEnd";
    addMsg(ENUM_MSG_REMIND, "RosApi", "RosApi", 8164, robotId );

    return returnResult;
}

//加载建图
int RosApi::loadMap(struct UserInfo userInfo, std::string mapFileName, int &returnResult)
{
    qDebug()<<"RosApi::loadMap"<<QString::fromStdString(mapFileName);
    if(0==mapFileName.size())
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8169, robotId );
        returnResult=-1;
        return 0;
    }


    if(1!=checkYamlMap(mapFileName))
    {
        returnResult=-2;
        return 0;
    }

    pthread_mutex_lock(&rosDataMutex);
    currentMapFileName=QString::fromStdString(mapFileName);
    pthread_mutex_unlock(&rosDataMutex);


    loadReflectMap(mapFileName);

    QString mapFilePath=D_MAP_PATH;
    mapFilePath+=currentMapFileName;

    //vision match load
    if(1!=loadMapFromYaml(mapFilePath))
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8169, robotId ,0,0,0,0,currentMapFileName);
        returnResult=-3;
        return 0;
    }

    switch(locationSolver)
    {
    case E_LOCATION_SOLVER_AMCL:
    {
        //屏蔽amcl保护一段时间
        #ifdef D_USE_AMCL_ADVANCE
        if(true==isUseAmclAdvance)
        {
            //屏蔽xｓ不做保护
            amclAdvance->setNoCheckCount(D_AMCL_NOCHECK_COUNT);//大地图加载导致激光接收超时
        }
        #endif
        //ros load
    //    addVitrualBlockFromMapFile(mapFilePath);//hualei 虚拟墙暂时不用
    //    int returnResult;
        std::string systemCommand="roslaunch tcr100 map_server.launch map:=";
        systemCommand+=mapFilePath.toStdString();
        systemCommand+=" &";
        map_to_load=systemCommand.c_str();
        int map_loader_status=system(map_to_load);
        if(map_loader_status==-1)
        {
            printf("map_loader system error... \n");
            returnResult=-4;
        }
        else
        {
            if(WIFEXITED(map_loader_status))
            {
                if(0==WEXITSTATUS(map_loader_status))
                {
                    printf("map_loader run successfully... \n");
                    returnResult=1;
                }
                else
                {
                    printf("map_loader run failed %d \n",WEXITSTATUS(map_loader_status));
                    returnResult=-5;
                }
            }
            else
            {
                printf("map_loader exit code %d \n",WEXITSTATUS(map_loader_status));
                returnResult=-6;
            }
        }
        break;
    }
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER:
    {

        int first = mapFilePath.lastIndexOf ("."); //从后面查找"/"位置
        QString title = mapFilePath.left (first); //从左边截取
        title+=".pbstream";
        qDebug()<<"77777777777 E_LOCATION_SOLVER_CARTOGRAPHER_LASER title"<<title;
        break;
        std::string systemCommand="roslaunch cartographer_ros localization_2d.launch load_state_filename:=";
        systemCommand+=title.toStdString();
        systemCommand+=" &";
        map_to_load=systemCommand.c_str();
        int map_loader_status=system(map_to_load);
        if(map_loader_status==-1)
        {
            printf("cart map_loader system error... \n");
            returnResult=-7;
        }
        else
        {
            if(WIFEXITED(map_loader_status))
            {
                if(0==WEXITSTATUS(map_loader_status))
                {
                    printf("cart map_loader run successfully... \n");
                    returnResult=1;
                }
                else
                {
                    printf("cart map_loader run failed %d \n",WEXITSTATUS(map_loader_status));
                    returnResult=-8;
                }
            }
            else
            {
                printf("cart map_loader exit code %d \n",WEXITSTATUS(map_loader_status));
                returnResult=-9;
            }
        }
        break;
    }
    default:
    {
        qDebug()<<"error2,unsupport location solver"<<locationSolver;
        return -1;
    }
    }

    return returnResult;
}

int RosApi::loadReflectMap(string mapFileName_yaml)
{

    QString  mapName=QString::fromStdString(mapFileName_yaml);
    qDebug()<<"RosApi::loadReflectMap "<<mapName;

    QRegExp rx("(.*).yaml");

    bool isMatch=rx.exactMatch(mapName);

    if(false==isMatch )
    {
        qDebug()<<"map file name error "<<mapName;
        return -1;
    }

    QString mapOriginalName=rx.cap(1);

    QString   filePath = D_MAP_PATH + mapOriginalName+".ref";
    return reflectPositionMatch->loadReflectMapFile(filePath);
}

//选择定位算法
int RosApi::setLocationSolver(struct UserInfo userInfo,enum E_LOCATION_SOLVER solverIn,int &returnFlag)
{
    locationSolver=solverIn;
    int returnResult;

    switch (solverIn) {
    case E_LOCATION_SOLVER_AMCL:
        localization_algorithm=0;
        returnResult=1;
        break;
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER:
        localization_algorithm=1;
        returnResult=1;
        break;
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER_IMU:
        localization_algorithm=2;
        returnResult=1;
        break;
    default:
        localization_algorithm=0;
        returnResult=0;
        break;
    }

    return returnResult;
}


//选择全局规划器
int RosApi::setTrajectoryGlobalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_GLOBAL_SOLVER solverIn,int &returnFlag)
{
    trajectoryGlobalSolver=solverIn;
    int returnResult;
    switch (solverIn) {
    case E_TRAJECTORY_GLOBAL_SOLVER_ASTAR:
        global_trajectory_selector=0;
        returnResult=1;
        break;
    case E_TRAJECTORY_GLOBAL_SOLVER_DIJKSTRAS:
        global_trajectory_selector=1;
        returnResult=1;
        break;
    default:
        global_trajectory_selector=0;
        returnResult=0;
        break;
    }
    return returnResult;
}

//选择局部规划器
int RosApi::setTrajectoryLocalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_LOCAL_SOLVER solverIn,int &returnFlag)
{
    trajectoryLocalSolver=solverIn;
    int returnResult;
    switch (solverIn) {
    case E_TRAJECTORY_LOCAL_SOLVER_DWA:
        local_trajectory_selector=0;
        returnResult=1;
        break;
    case E_TRAJECTORY_LOCAL_SOLVER_BASELOCAL:
        local_trajectory_selector=1;
        returnResult=1;
        break;
    case E_TRAJECTORY_LOCAL_SOLVER_TEB:
        local_trajectory_selector=2;
        returnResult=1;
        break;
    default:
        local_trajectory_selector=0;
        returnResult=0;
        break;
    }
    return returnResult;
}

//start 导航
int RosApi::startNavigation(struct UserInfo userInfo,int &returnFlag)//start navigation
{
//    //debug
//    return 1;

    int returnResult;
    //start the localization algorithm
    std::string navigation_algorithm;
    if(localization_algorithm==0)
    {
        returnResult=1;
        if(global_trajectory_selector==0 && local_trajectory_selector==0)
        {
            ROS_INFO("astar and dwa is choosed \n");
            navigation_algorithm="roslaunch tcr100 move_base_astar_daw.launch &";
        }
    }
    const char* navigation_algorithm_sh=navigation_algorithm.c_str();
    int start_navigation_status=system(navigation_algorithm_sh);
    if(start_navigation_status==-1)
    {
        printf("start_navigation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(start_navigation_status))
        {
            if(0==WEXITSTATUS(start_navigation_status))
            {
                printf("start_navigation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("start_navigation run failed %d \n",WEXITSTATUS(start_navigation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("start_navigation exit code %d \n",WEXITSTATUS(start_navigation_status));
            returnResult=0;
        }
    }

    return returnResult;
}

void RosApi::startNavigation_bySignal()
{
//    emit startNavigation_signal();
}
//stop 导航
int RosApi::stopNavigation(struct UserInfo userInfo,int &returnFlag)//start navigation
{
    int returnResult;
    std::string stop_navigation=D_SH_FILE_PATH;
    std::string file_path;
    file_path="stop_navigation.sh &";
    stop_navigation+=file_path;
    const char* stop_navigation_sh=stop_navigation.c_str();
    int stop_navigation_status=system(stop_navigation_sh);
    if(stop_navigation_status==-1)
    {
        printf("stop_navigation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(stop_navigation_status))
        {
            if(0==WEXITSTATUS(stop_navigation_status))
            {
                printf("stop_navigation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("stop_navigation run failed %d \n",WEXITSTATUS(stop_navigation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("stop_navigation exit code %d \n",WEXITSTATUS(stop_navigation_status));
            returnResult=0;
        }
    }
    return returnResult;
}

void RosApi::moveToPosition_bySignal(VehiclePosition positionIn, double velocity)
{
//    emit moveToPosition_signal(positionIn,velocity);
}

//获取agv位置
int RosApi::getVehiclePosition_ros(struct UserInfo userInfo, PositionInfo &positionInfoOut)
{

    positionInfoOut=vehiclePositionInfo_ros;
    return 0;
}

int RosApi::getVehiclePosition_pclicp(UserInfo userInfo, PositionInfo &positionInfoOut)
{
    positionInfoOut=vehiclePositionInfo_pclicp;
    return 0;
}

int RosApi::getVehiclePositionInfo_vmark(UserInfo userInfo, PositionInfo &positionInfoOut)
{
    positionInfoOut=vehiclePositionInfo_vmark;
    return 0;
}

int RosApi::getVehiclePositionInfo_reflect(UserInfo userInfo, PositionInfo &positionInfoOut)
{
    positionInfoOut=vehiclePositionInfo_reflect;
    return 0;
}

int RosApi::updateVehiclePositionOnce()
{
    //时间分析
    struct timespec startTime,endTime1,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;

    laserDriverLoseDataCount[0]++;
    laserDriverLoseDataCount[1]++;
//    static int tick=0;
    setWheelOdometerData(encoder_odemeterData);

    clock_gettime(CLOCK_MONOTONIC, &endTime1);
    currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"------RosApi::updateVehiclePositionOnce rosTime1:"<<currentConsumed;
    }

    if(ros::ok())
    {
//        tick++;
//        qDebug()<<"RosApi::updateVehiclePositionOnce tick"<<tick;
        isRosLoopFinished=false;
        //激光数据驱动
//        sickTimDriverLoopOnce();//sick laser可能不能允许睡
        #ifdef D_USE_INTERNAL_LASER_DRIVER
        for(int i=0;i<internalLaserScanDriver.size();i++)
        {
            int tmpResult=internalLaserScanDriver[i]->publishLaserDataLoopOnce();
            if(1!=tmpResult)
            {
                qDebug()<<"error, RosApi publishLaserDataLoopOnce";
                addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8162, robotId ,i);
            }
            if(false==isUseRosLaserMsg)
            {
                if((*laserScanDevice_infoList)[i].laserScanName=="front_scan")
                {
                    sensor_msgs::LaserScan laserSensorDataOut;
                    int tmpResult=internalLaserScanDriver[i]->getLaserSensorData(laserSensorDataOut);
                    internalLaserDriverStatus[0]=internalLaserScanDriver[i]->getStatus();
                    if(1==tmpResult)
                    {
                        sensor_msgs::LaserScan* tmpSensorData=new sensor_msgs::LaserScan;
                        *tmpSensorData=laserSensorDataOut;
                        sensor_msgs::LaserScanConstPtr tmp_laserSensorData(tmpSensorData);
                        frontLaserScanDataCallback(tmp_laserSensorData);
                    }

                }
                else if((*laserScanDevice_infoList)[i].laserScanName=="back_scan")
                {
                    sensor_msgs::LaserScan laserSensorDataOut;
                    int tmpResult=internalLaserScanDriver[i]->getLaserSensorData(laserSensorDataOut);
                    internalLaserDriverStatus[1]=internalLaserScanDriver[i]->getStatus();
                    if(1==tmpResult)
                    {
                        sensor_msgs::LaserScan* tmpSensorData=new sensor_msgs::LaserScan;
                        *tmpSensorData=laserSensorDataOut;
                        sensor_msgs::LaserScanConstPtr tmp_laserSensorData(tmpSensorData);
                        backLaserScanDataCallback(tmp_laserSensorData);
                    }

                }

            }
        }
        #endif

        broadcastStaticTransform();

        clock_gettime(CLOCK_MONOTONIC, &endTime1);
        currentConsumed=1000000000*(endTime2.tv_sec-startTime.tv_sec)+endTime2.tv_nsec-startTime.tv_nsec;
        currentConsumed=currentConsumed/1000.0;
        if(100==debugFlag)
        {
            qDebug()<<"------RosApi::updateVehiclePositionOnce rosTime2:"<<currentConsumed;
        }


        //amcl计算，tf获取位置
        ros::spinOnce();//可能耗时很久，第一次初始化地图位置的时候，特别是大地图。//点云转换耗时13ms

        clock_gettime(CLOCK_MONOTONIC, &endTime3);
        currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
        currentConsumed=currentConsumed/1000.0;
        if(100==debugFlag)
        {
            qDebug()<<"------RosApi::updateVehiclePositionOnce rosTime3:"<<currentConsumed;
        }

//        qDebug()<<"ros::spinOnce()";
        if(isUseAmclAdvance)
        {
            if(debugFlag)
            {
                qDebug()<<"amclAdvanceLoopOnce()";
            }
            amclAdvanceLoopOnce();//可能耗时很久？不会很久但是有失败可能性。
    //        getVehiclePositionFrom_tf();//放在这里可能会被amcl节点卡住，延迟几百毫秒
         }

        if(isUseVmarkDetect)
        {
            //vmark,ros定时器提高到２０ｍｓ,vｍａｒｋ执行频率降低３倍。
           updateVmarkPosition();//一堵墙时，很消耗时间，把该运算迁移到了ｒｏｓ定时器

        }


        if(isUseReflectDetect)
        {
            //reflector
            updateReflectPosition();//

        }


        //icp
        updatePciIcpPosition();

        //激光里程计更新，
        if(E_ODOM_SOLVER_LASER==odomSolver)
        {
            #ifdef D_USE_LASER_ODOMETRY
            laserOdometry->laserOdometryLoopOnce();
            #endif
        }


    }
    else
    {
        isRosLoopFinished=true;
    }

    //１）傲视驱动时，出现丢失超过２１，可能是ｃｐｕ消耗过高引起的。
    checkLaserDriverLoseData();

    clock_gettime(CLOCK_MONOTONIC, &endTime4);
    currentConsumed=1000000000*(endTime4.tv_sec-startTime.tv_sec)+endTime4.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(100==debugFlag)
    {
        qDebug()<<"------RosApi::updateVehiclePositionOnce rosTime4:"<<currentConsumed;
    }

}

//设置agv初始位置
int RosApi::setVehiclePosition(struct UserInfo userInfo, struct VehiclePosition positionIn,
                               double coveRatioIn, int &returnResult)
{
    qDebug()<<"RosApi::setVehiclePosition x"<<positionIn.x<<" ,y"<<positionIn.y<<" ,r"<<positionIn.rotate
           <<"coveRatioIn"<<coveRatioIn;
//    int returnResult;
    reflectPositionMatch->setInitialVehiclePosition(positionIn);

    if(isUseAmclAdvance)
    {
        amclAdvance->forceInitialAgvPosition(amclInitialPoseCalculateCount);
    }

    geometry_msgs::PoseWithCovarianceStamped init_pose;
    for(int i=0; i<36; i++)
    {
      init_pose.pose.covariance[i]=0;
    }
    init_pose.pose.covariance[0]=setPosition_cov_xy*coveRatioIn;//rviz设定初始位置使用的协方差(0.25,0.25,0.068)
    init_pose.pose.covariance[7]=setPosition_cov_xy*coveRatioIn;
    init_pose.pose.covariance[35]=setPosition_cov_r*coveRatioIn;

    int count=0;
    while(ros::ok()&&count<3)
    {
        rosRate.sleep();
//        ros::spinOnce();
        init_pose.header.frame_id = "map";
        init_pose.header.stamp = ros::Time::now();
        init_pose.pose.pose.position.x =positionIn.x ;
        init_pose.pose.pose.position.y =positionIn.y ;
        geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(positionIn.rotate);
        init_pose.pose.pose.orientation =pose_quat ;
//        ROS_INFO("initial position: %.2f, %.2f, %.2f",
//                 init_pose.pose.pose.position.x, init_pose.pose.pose.position.y,
//                 tf::getYaw(init_pose.pose.pose.orientation));
        initial_position_pub_->publish(init_pose);
        returnResult=1;
        count++;
    }
    return returnResult;
}

//设置失败重试次数
int RosApi::setReplanCoutLimitWhenFailure(struct UserInfo userInfo,int countLimit,int &returnFlag)
{
    replanCountLimit=countLimit;
    int returnResult;
    std::string faile_retry_str;
    stringstream faile_retry_ss;
    faile_retry_ss<<countLimit;
    faile_retry_ss>>faile_retry_str;
    std::string faile_retry_file="rosrun tcr100 nav_state.py _repeat_num:=";
    faile_retry_file+=faile_retry_str;
    faile_retry_file+=" &";
    const char* faile_retry_sh=faile_retry_file.c_str();
    int faile_retry_status=system(faile_retry_sh);
    if(faile_retry_status==-1)
    {
        printf("faile_retry system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(faile_retry_status))
        {
            if(0==WEXITSTATUS(faile_retry_status))
            {
                printf("faile_retry run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("faile_retry run failed %d \n",WEXITSTATUS(faile_retry_status));
                returnResult=0;
            }
        }
        else
        {
            printf("faile_retry exit code %d \n",WEXITSTATUS(faile_retry_status));
            returnResult=0;
        }
    }

    return returnResult;
}

//导航到指定点
//int RosApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
//{
//    qDebug()<<"RosApi::moveToPosition";
//    //setVelocity(velocity);
//    sendPositionMessage(positionIn);

//    return 0;
//}


//导航到指定点
//int RosApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
//{
//    tcr100::VehicleGoal msg;
//    setVelocity(velocity);
//    msg.position_x=positionIn.x;
//    msg.position_y=positionIn.y;
//    msg.position_rotate=positionIn.rotate;
//    goal_pub_.publish(msg);

//    return 0;
//}

//导航到指定点
int RosApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag)
{
    int returnResult;
    bool goalReached=false;
    while ((move_base_client_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
    {
      ROS_WARN_THROTTLE(1, "Waiting for move_base action server to come up...");
    }
    setVelocity(velocity);//todo could have problem ?
    if(goalReached=moveToGoal(positionIn))
    {
        returnResult=1;
//        ros::spinOnce();
    }
    else
    {
        returnResult=0;
    }
    return returnResult;
}
//int RosApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,
//                           int &returnFlag)
//{

////    QDesktopServices ds;

////    if (!ds.openUrl(QUrl::fromLocalFile(fileList.at(0))))

////        showMessage(QStringLiteral("pdf程序不存在"));

//    int returnResult;
////   setVelocity(velocity);

//   std::string x_str;
//   stringstream x_ss;
//   x_ss<<positionIn.x;
//   x_ss>>x_str;
//   std::string y_str;
//   stringstream y_ss;
//   y_ss<<positionIn.y;
//   y_ss>>y_str;
//   std::string r_str;
//   stringstream r_ss;
//   r_ss<<positionIn.rotate;
//   r_ss>>r_str;

//   std::string goal_file="/home/tcr/tcr100_ws/src/tcr100/scripts/goal_sending.py ";
//   goal_file+=x_str;
//   goal_file+=" ";
//   goal_file+=y_str;
//   goal_file+=" ";
//   goal_file+=r_str;
//   goal_file+=" &";
////   const char* goal_sh=goal_file.c_str();
////   int set_goal_status=system(goal_sh);
////   QString tmpStr="./";
//   QString program = "/home/tcr/tcr100_ws/devel/lib/tcr100/goalcmd ";
////    QString program = "/home/tcr/tcr100_ws/devel/lib/tcr100/odom.sh ";
////  QString program = QString::fromStdString(goal_file);
//  QStringList arguments;
//  QString str;
//  arguments << str;
////  QProcess *myProcess = new QProcess();
////  myProcess->start(program, arguments);

//  QProcess::startDetached(program);

////   printf("goal sending command: %s \n",goal_sh);
////   if(set_goal_status==-1)
////   {
////       printf("set_velocity system error... \n");
////       returnResult=0;
////   }
////   else
////   {
////       if(WIFEXITED(set_goal_status))
////       {
////           if(0==WEXITSTATUS(set_goal_status))
////           {
////               printf("set_goal run successfully... \n");
////               returnResult=1;
////           }
////           else
////           {
////               printf("set_goal run failed %d \n",WEXITSTATUS(set_goal_status));
////               returnResult=0;
////           }
////       }
////       else
////       {
////           printf("set_velocity exit code %d \n",WEXITSTATUS(set_goal_status));
////           returnResult=0;
////       }
////   }
//   return returnResult;
//}
//导航到指定点
//int RosApi::moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,
//                           int &returnFlag)
//{
////    int result;
////    result=spawnl(P_WAIT,"",NULL);
////  int returnResult;
//    setVelocity(velocity);

//    std::string x_str;
//    stringstream x_ss;
//    x_ss<<positionIn.x;
//    x_ss>>x_str;
//    std::string y_str;
//    stringstream y_ss;
//    y_ss<<positionIn.y;
//    y_ss>>y_str;
//    std::string r_str;
//    stringstream r_ss;
//    r_ss<<positionIn.rotate;
//    r_ss>>r_str;

//    std::string goal_file="/home/tcr/tcr100_ws/src/tcr100/scripts/goal_sending.py ";
//    goal_file+=x_str;
//    goal_file+=" ";
//    goal_file+=y_str;
//    goal_file+=" ";
//    goal_file+=r_str;
//    goal_file+=" &";
//    const char* goal_sh=goal_file.c_str();
//    int set_goal_status=system(goal_sh);
//    printf("goal sending command: %s \n",goal_sh);
//    if(set_goal_status==-1)
//    {
//        printf("set_velocity system error... \n");
//        returnResult=0;
//    }
//    else
//    {
//        if(WIFEXITED(set_goal_status))
//        {
//            if(0==WEXITSTATUS(set_goal_status))
//            {
//                printf("set_goal run successfully... \n");
//                returnResult=1;
//            }
//            else
//            {
//                printf("set_goal run failed %d \n",WEXITSTATUS(set_goal_status));
//                returnResult=0;
//            }
//        }
//        else
//        {
//            printf("set_velocity exit code %d \n",WEXITSTATUS(set_goal_status));
//            returnResult=0;
//        }
//    }
//    return returnResult;
//}

//多点巡航
int RosApi::moveFollowPath(struct UserInfo userInfo,std::string filePath,
                           std::string trajectoryName,double nearbyDistance,
                           double velocity,int &returnFlag)
{
    int returnResult;
    setVelocity(velocity);
    setNearbyDistance(nearbyDistance);


    return returnResult;
}

//设置速度
int RosApi::changeVelocity(struct UserInfo userInfo,double velocity,int &returnFlag)
{
    int returnResult;
    if(setVelocity(velocity)==1)
    {
        returnResult=1;
    }
    else
    {
        returnResult=0;
    }
    return returnResult;
}



//获取导航状态
int RosApi::getMovingStatus(struct UserInfo userInfo,enum E_GOAL_STATUS& status)
{
    switch(nav_status)
    {
    case 0:
        status=E_GOAL_STATUS_PENDING;
        break;
    case 1:
        status=E_GOAL_STATUS_ACTIVE;
        break;
    case 2:
        status=E_GOAL_STATUS_PREEMPTED;
        break;
    case 3:
        status=E_GOAL_STATUS_SUCCEEDED;
        break;
    case 4:
        status=E_GOAL_STATUS_ABORTED;
        break;
    case 5:
        status=E_GOAL_STATUS_REJECTED;
        break;
    case 6:
        status=E_GOAL_STATUS_PREEMPTING;
        break;
    case 7:
        status=E_GOAL_STATUS_RECALLING;
        break;
    case 8:
        status=E_GOAL_STATUS_RECALLED;
        break;
    case 9:
        status=E_GOAL_STATUS_LOST;
        break;
    default:
        status=E_GOAL_STATUS_PENDING;
        break;
    }
    return 0;
}

//获取剩余时间
int RosApi::getRemainningTime(struct UserInfo userInfo,double &timeOut)
{
    int returnResult;
    returnResult=1;
    timeOut=path_lenght/rosVehicleCmdVel.x_move;
    return returnResult;
}

//获取剩余距离
int RosApi::getRemainningDistance(struct UserInfo userInfo,double &distanceOut)
{
    int returnResult;
    returnResult=1;
    distanceOut=path_lenght;
    return returnResult;
}

//取消当前导航目标
int RosApi::cancelMotion(struct UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_goal;
    cancel_goal="rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID {} ";
    const char* cancel_goal_sh=cancel_goal.c_str();
    int status=system(cancel_goal_sh);
    if(status==-1)
    {
        printf("cancel goal system error... \n");
    }
    else
    {
        if(WIFEXITED(status))
        {
            if(0==WEXITSTATUS(status))
            {
                printf("cancel goal run successfully... \n");
                nav_status=0;
                returnResult=1;
            }
            else
            {
                printf("cancel goal run failed %d \n",WEXITSTATUS(status));
            }
        }
        else
        {
            printf("cancel goal exit code %d \n",WEXITSTATUS(status));
        }
    }
    return returnResult;
}
//设置导航允许误差
int RosApi::setPositionArrivedAccuracy(struct UserInfo userInfo,
                                       double distanceErrorIn,double angularErrorIn,
                                       int &returnFlag)
{
    arrivedAccuracy_linear=distanceErrorIn;
    arrivedAccuracy_angular=angularErrorIn;
    int returnResult;
    std::string distance_str,yaw_str;
    stringstream dis_ss,yaw_ss;
    dis_ss<<distanceErrorIn;
    dis_ss>>distance_str;

    yaw_ss<<angularErrorIn;
    yaw_ss>>yaw_str;
    std::string distance_file=D_SH_FILE_PATH;
    std::string file_path="change_distance_accuracy.sh ";
    distance_file+=file_path;
    distance_file+=distance_str;
    distance_file+=" ";
    distance_file+=yaw_str;
    distance_file+=" &";
    const char* distance_sh=distance_file.c_str();
    int distance_status=system(distance_sh);
    if(distance_status==-1)
    {
        printf("change location accuracy system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(distance_status))
        {
            if(0==WEXITSTATUS(distance_status))
            {
                printf("change location accuracy run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("change location accuracy run failed %d \n",WEXITSTATUS(distance_status));
                returnResult=0;
            }
        }
        else
        {
            printf("change location accuracy exit code %d \n",WEXITSTATUS(distance_status));
            returnResult=0;
        }
    }
    return returnResult;
}


//设置局部地图大小
int RosApi::setLocalPlanRange(struct UserInfo userInfo,double x_rangeIn,
                              double y_rangeIn,int &returnFlag)
{
    localPlanXRange=x_rangeIn;
    localPlanYRange=y_rangeIn;
    int returnResult;
    std::string param_str[4];
    stringstream param_ss[3];
    param_ss[0]<<x_rangeIn;
    param_ss[1]<<y_rangeIn;
    for(int j=0;j<2;j++)
    {
        param_ss[j]>>param_str[j];
    }
    std::string local_range_file=D_SH_FILE_PATH;
    std::string file_path="local_range.sh ";
    local_range_file+=file_path;
    for(int i=0;i<2;i++)
    {
        if(i!=0)
        {
            local_range_file+=" ";
        }
        local_range_file+=param_str[i];
    }
    local_range_file+=" &";
    const char* local_range_sh=local_range_file.c_str();
    int local_range_status=system(local_range_sh);
    if(local_range_status==-1)
    {
        printf("set local_range system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(local_range_status))
        {
            if(0==WEXITSTATUS(local_range_status))
            {
                printf("set local_range run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set local_range run failed %d \n",WEXITSTATUS(local_range_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set local_range exit code %d \n",WEXITSTATUS(local_range_status));
            returnResult=0;
        }
    }
    return returnResult;
}

int RosApi::setVehicleBorder(QString configFileName)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFileName, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, setVehicleBorder open file error！";
       return -1;
    }

    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "BasicSetting", ok );

    QDomNodeList devNodes = inNode.childNodes();
    std::vector<double>point_x;
    std::vector<double>point_y;
    point_x.resize(devNodes.size());
    point_y.resize(devNodes.size());

    for(int i=0;i<devNodes.size()-1;i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "x_position", ok ), point_x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "y_position", ok ), point_y[i] );
        if(!ok)
        {
           return -100-i;
        }
    }
    xmlParser.closeXml();
    setVehicleBorder(myUserInfo,point_x,point_y,myReturnKey);
    return 1;
}

//设置车体的footprint
int RosApi::setVehicleBorder(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag)
{
    //todo save file
    saveCarBorder(point_x,point_y);
    int returnResult;
    int size=point_x.size();
    int length=6;
    std::string param_str[100];
    stringstream param_ss[100];
    for(int j=0;j<size;j++)
    {
        if(j!=0)
        {
            param_ss[j*length-1]<<",";
        }
        for(int i=0;i<length-1;i++)
        {
            if(i==0)
            {
                param_ss[i+j*length]<<"[";
            }
            if(i==1)
            {
                param_ss[i+j*length]<<point_x[j];
            }
            if(i==2)
            {
                param_ss[i+j*length]<<",";
            }
            if(i==3)
            {
                param_ss[i+j*length]<<point_y[j];
            }  if(i==4)
            {
                param_ss[i+j*length]<<"]";
            }
        }
    }
    param_ss[size*length-1]<<"]";
    for(int k=0;k<=size*length-1;k++)
    {
        param_ss[k]>>param_str[k];
    }
    std::string footprint_file="rosrun dynamic_reconfigure dynparam set /move_base/local_costmap footprint '[";
    for(int m=0;m<=size*length;m++)
    {
        footprint_file+=param_str[m];
    }
    footprint_file+="'";
    footprint_file+=" &";
    const char* footprint_sh=footprint_file.c_str();
    printf("command: %s \n",footprint_sh);
    int footprint_status=system(footprint_sh);
    if(footprint_status==-1)
    {
        printf("set footprint system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(footprint_status))
        {
            if(0==WEXITSTATUS(footprint_status))
            {
                printf("set footprint run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set footprint run failed %d \n",WEXITSTATUS(footprint_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set footprint exit code %d \n",WEXITSTATUS(footprint_status));
            returnResult=0;
        }
    }
    return returnResult;
}


//设置local膨胀区大小
int RosApi::setLocalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag)
{
    localBarrierOffset=offsetIn;
    int returnResult;
    std::string local_inflation_str;
    stringstream local_inflation_ss;
    local_inflation_ss<<offsetIn;
    local_inflation_ss>>local_inflation_str;
    std::string local_inflation_file="rosrun dynamic_reconfigure dynparam set /move_base/local_costmap/inflation_layer inflation_radius ";
    local_inflation_file+=local_inflation_str;
    local_inflation_file+=" &";
    const char* local_inflation_sh=local_inflation_file.c_str();
    int local_inflation_status=system(local_inflation_sh);
    if(local_inflation_status==-1)
    {
        printf("set local_inflation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(local_inflation_status))
        {
            if(0==WEXITSTATUS(local_inflation_status))
            {
                printf("set local_inflation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set local_inflation run failed %d \n",WEXITSTATUS(local_inflation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set local_inflation exit code %d \n",WEXITSTATUS(local_inflation_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//设置global膨胀区大小
int RosApi::setGlobalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag)
{
    globalBarrierOffset=offsetIn;
    int returnResult;
    std::string global_inflation_str;
    stringstream global_inflation_ss;
    global_inflation_ss<<offsetIn;
    global_inflation_ss>>global_inflation_str;
    std::string global_inflation_file="rosrun dynamic_reconfigure dynparam set /move_base/global_costmap/inflation_layer inflation_radius ";
    global_inflation_file+=global_inflation_str;
    global_inflation_file+=" &";
    const char* global_inflation_sh=global_inflation_file.c_str();
    int global_inflation_status=system(global_inflation_sh);
    if(global_inflation_status==-1)
    {
        printf("set global_inflation system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(global_inflation_status))
        {
            if(0==WEXITSTATUS(global_inflation_status))
            {
                printf("set global_inflation run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set global_inflation run failed %d \n",WEXITSTATUS(global_inflation_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set global_inflation exit code %d \n",WEXITSTATUS(global_inflation_status));
            returnResult=0;
        }
    }
    return returnResult;
}
////重置
//int RosGuiApi::resetAll(struct UserInfo userInfo,int &returnFlag)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}
////获取ROS包的状态
//int RosGuiApi::getRosPackageStatus(enum E_ROS_PACKAGE package,enum E_ROS_PACKAGE_STATUS &status)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}

int RosApi::addVitrualBlock(struct UserInfo userInfo,std::string configFilePath,int &returnFlag)
{
    int returnResult;
    std::string virtual_wall_str;
    stringstream virtual_wall_ss;
    virtual_wall_ss<<configFilePath;
    virtual_wall_ss>>virtual_wall_str;
    std::string virtual_wall_file="roslaunch yocs_virtual_sensor standalone.launch virtual_wall:=";
    virtual_wall_file+=virtual_wall_str;
    virtual_wall_file+=" &";
    const char* virtual_wall_sh=virtual_wall_file.c_str();
    int virtual_wall_status=system(virtual_wall_sh);
    if(virtual_wall_status==-1)
    {
        printf("set virtual_wall system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(virtual_wall_status))
        {
            if(0==WEXITSTATUS(virtual_wall_status))
            {
                printf("set virtual_wall run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set virtual_wall run failed %d \n",WEXITSTATUS(virtual_wall_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set virtual_wall exit code %d \n",WEXITSTATUS(virtual_wall_status));
            returnResult=0;
        }
    }
    return returnResult;
}

int RosApi::deleteVitrualBlock(struct UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_virtual_wall=D_SH_FILE_PATH;
    std::string file_path;
    file_path="delete_virtual_wall.sh &";
    cancel_virtual_wall+=file_path;
    const char* cancel_virtual_wall_sh=cancel_virtual_wall.c_str();
    int cancel_virtual_wall_status=system(cancel_virtual_wall_sh);
    if(cancel_virtual_wall_status==-1)
    {
        printf("cancel_virtual_wall system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(cancel_virtual_wall_status))
        {
            if(0==WEXITSTATUS(cancel_virtual_wall_status))
            {
                printf("cancel_virtual_wall run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("cancel_virtual_wall run failed %d \n",WEXITSTATUS(cancel_virtual_wall_status));
                returnResult=0;
            }
        }
        else
        {
            printf("cancel_virtual_wall exit code %d \n",WEXITSTATUS(cancel_virtual_wall_status));
            returnResult=0;
        }
    }

    return returnResult;
}

//start waypoints follow
int RosApi::startPathFollowMotion(UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    int waypoints_follow_status=system("rosrun yocs_waypoints nav_ctrl_pub.py &");
    if(waypoints_follow_status==-1)
    {
        printf("waypoints_follow start system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(waypoints_follow_status))
        {
            if(0==WEXITSTATUS(waypoints_follow_status))
            {
                printf("waypoints_follow start run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("waypoints_follow start run failed %d \n",WEXITSTATUS(waypoints_follow_status));
                returnResult=0;
            }
        }
        else
        {
            printf("waypoints_follow start exit code %d \n",WEXITSTATUS(waypoints_follow_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//pause motion
int  RosApi::ceaseMotion(UserInfo userInfo,int &returnFlag)
{
    int returnResult;
    std::string cancel_goal;
    cancel_goal="rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID {} ";
    const char* cancel_goal_sh=cancel_goal.c_str();
    int status=system(cancel_goal_sh);
    if(status==-1)
    {
        printf("cancel goal system error... \n");
    }
    else
    {
        if(WIFEXITED(status))
        {
            if(0==WEXITSTATUS(status))
            {
                printf("cancel goal run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("cancel goal run failed %d \n",WEXITSTATUS(status));
            }
        }
        else
        {
            printf("cancel goal exit code %d \n",WEXITSTATUS(status));
        }
    }
    return returnResult;

}


int RosApi::startMotion(UserInfo userInfo,int &returnFlag)
{
    current_goal->target_pose.header.frame_id = "map";
    current_goal->target_pose.header.stamp = ros::Time::now();
    ROS_INFO("Continue the pevious goal location ...");
    move_base_client_.sendGoal(*current_goal);
    move_base_client_.waitForResult();
    if(move_base_client_.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Goal have finally been reached!");
        return true;
    }
    else
    {
        ROS_INFO("The robot failed to reach complete the cotinune navigation");
        return false;
    }

}
//int RosGuiApi::addForbidenZone(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag)
//{
//    int returnResult;

//    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
//    {
//        //qDebug()<<"vehicleAuto sucess!";
//        returnResult= 1;
//    }
//    else
//    {
//        //qDebug()<<"vehicleAuto error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}



//**********functions defined by myself*************//
void RosApi::moveBaseResultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr& msg)
{
    if (!msg->status_list.empty())
    {
        actionlib_msgs::GoalStatus goalStatus=msg->status_list[0];
        ROS_INFO("got goal state!");
        if(goalStatus.status==1)
        {
            ROS_INFO("goal started!");
            nav_status=1;
        }
        else if(goalStatus.status==2)
        {
            ROS_INFO("goal preemmpted!");
            nav_status=2;
        }
        else if(goalStatus.status==3)
        {
            ROS_INFO("goal succeeded!");
            nav_status=3;
        }
        else if(goalStatus.status==4)
        {
            ROS_INFO("goal aborted!");
            nav_status=4;
        }
        else if(goalStatus.status==5)
        {
            ROS_INFO("goal rejected!");
            nav_status=5;
        }
        else if(goalStatus.status==6)
        {
            ROS_INFO("goal preempting!");
            nav_status=6;
        }
        else if(goalStatus.status==7)
        {
            ROS_INFO("goal recalling!");
            nav_status=7;
        }
        else if(goalStatus.status==8)
        {
            ROS_INFO("goal recalled!");
            nav_status=8;
        }
        else if(goalStatus.status==9)
        {
            ROS_INFO("goal lost!");
            nav_status=9;
        }
    }
    else
    {
//        ROS_INFO("goal pending!");
        nav_status=0;
    }
}

void RosApi::amcl_match_scoreCallback(const amcl::AmclExtensionConstPtr &msg)
{
    amcl_match_score=msg->weight;
    amcl_pos_cov=msg->pos_cov;
}


//set velocity
int RosApi::setVelocity(double velocity)
{
    currentSettingVelocity=velocity;
    int returnResult;
    std::string str;
    stringstream ss;
    ss<<velocity;
    ss>>str;
    //std::string velocity_file=D_SH_FILE_PATH;
    std::string velocity_file="rosrun dynamic_reconfigure dynparam set /move_base/DWAPlannerROS min_vel_x ";
    //velocity_file+=file;
    velocity_file+=str;
    velocity_file+=" &";
    const char* velocity_sh=velocity_file.c_str();
    int set_velocity_status=system(velocity_sh);
    printf("vel change command %s \n",velocity_sh);
    if(set_velocity_status==-1)
    {
        printf("set_velocity system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(set_velocity_status))
        {
            if(0==WEXITSTATUS(set_velocity_status))
            {
                printf("set_velocity run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("set_velocity run failed %d \n",WEXITSTATUS(set_velocity_status));
                returnResult=0;
            }
        }
        else
        {
            printf("set_velocity exit code %d \n",WEXITSTATUS(set_velocity_status));
            returnResult=0;
        }
    }
    return returnResult;
}


int RosApi::setNearbyDistance(double distance)
{
    int returnResult;
    std::string nearby_distance_str;
    stringstream nearby_distance_ss;
    nearby_distance_ss<<distance;
    nearby_distance_ss>>nearby_distance_str;
    //std::string velocity_file=D_SH_FILE_PATH;
    std::string nearby_distance_file="rosrun dynamic_reconfigure dynparam set /waypoints_navi close_enough ";
    //velocity_file+=file;
    nearby_distance_file+=nearby_distance_str;
    nearby_distance_file+=" &";
    const char* nearby_distance_sh=nearby_distance_file.c_str();
    int nearby_distance_status=system(nearby_distance_sh);
    if(nearby_distance_status==-1)
    {
        printf("nearby_distance system error... \n");
        returnResult=0;
    }
    else
    {
        if(WIFEXITED(nearby_distance_status))
        {
            if(0==WEXITSTATUS(nearby_distance_status))
            {
                printf("nearby_distance run successfully... \n");
                returnResult=1;
            }
            else
            {
                printf("nearby_distance run failed %d \n",WEXITSTATUS(nearby_distance_status));
                returnResult=0;
            }
        }
        else
        {
            printf("nearby_distance exit code %d \n",WEXITSTATUS(nearby_distance_status));
            returnResult=0;
        }
    }
    return returnResult;
}

//Move to a signed position
bool RosApi::moveToGoal(struct VehiclePosition positionIn)
{
    move_base_msgs::MoveBaseGoal goal;

    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x =positionIn.x ;
    goal.target_pose.pose.position.y =positionIn.y ;
    geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(positionIn.rotate);
    goal.target_pose.pose.orientation =pose_quat ;
    ROS_INFO("New goal: %.2f, %.2f, %.2f",
             goal.target_pose.pose.position.x, goal.target_pose.pose.position.y,
             tf::getYaw(goal.target_pose.pose.orientation));
    ROS_INFO("sending goal location ...");
    move_base_client_.sendGoal(goal);
//    move_base_client_.waitForResult();
//    if(move_base_client_.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
//    {
//        ROS_INFO("Goal have been reached!");
//        return true;
//    }
//    else
//    {
//        ROS_INFO("The robot failed to reach the destination");
//        return false;
//    }

}

//Get odometry from encoder
int RosApi::setWheelOdometerData(OdemeterData odemeterDataIn)
{
    if(E_ODOM_SOLVER_ENCODER!=odomSolver)
    {
        return -1;
    }

//    qDebug()<<"1111111111 encoder odom before  "<<odemeterDataIn.pos.x
//           <<odemeterDataIn.pos.y
//           <<odemeterDataIn.pos.rotate;
//    //位置补偿处理
//    KDL::Rotation tmpRotation;
//    tmpRotation=tmpRotation.RPY(0,0,odemeterDataIn.pos.rotate);
//    KDL::Vector tmpVector(odemeterDataIn.pos.x,odemeterDataIn.pos.y,0);
//    KDL::Frame frame_origin_encoderOdom_tmp(tmpRotation,tmpVector);
//    (*frame_origin_encoderOdom)=frame_origin_encoderOdom_tmp;

//    KDL::Frame frame_publishOdome=(*frame_origin_encoderOdom)*(*frame_compensation_encoderOdom);

//    double tmp_roll,tmp_pitch;
//    odemeterDataIn.pos.x=frame_publishOdome.p[0];
//    odemeterDataIn.pos.y=frame_publishOdome.p[1];
//    frame_publishOdome.M.GetRPY(tmp_roll,tmp_pitch,odemeterDataIn.pos.rotate);
//    //速度方向补偿？

//    odemeterFinalData_encoder=odemeterDataIn.pos;
//    qDebug()<<"11111111111 encoder odom after "<<odemeterDataIn.pos.x
//           <<odemeterDataIn.pos.y
//           <<odemeterDataIn.pos.rotate<<" isEncoderOdomPublish="<<isEncoderOdomPublish;


    if(false==isEncoderOdomPublish)
    {
        return 0;
    }

    static tf::TransformBroadcaster odom_broadcaster;

    if(ros::ok())
    {

        ros::Time current_time = ros::Time::now();
        //发布odom tf,cartographer不需要发布。amcl必须用,gmapping必须用。
        if( (E_MAP_SOLVER_GMAPPING==mapSolver && true==isInCreatingMap)
                || (E_LOCATION_SOLVER_AMCL==locationSolver &&  false==isInCreatingMap))
        {
            //compute odometry in a typical way given the velocities of the robot
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odemeterDataIn.pos.rotate);
            //first, we'll publish the transform over tf
            geometry_msgs::TransformStamped odom_trans;
            geometry_msgs::PoseWithCovariance odom_covariance;
            odom_trans.header.stamp = current_time;
            odom_trans.header.frame_id = "odom";
            odom_trans.child_frame_id = "base_link";

            odom_trans.transform.translation.x = odemeterDataIn.pos.x;
            odom_trans.transform.translation.y = odemeterDataIn.pos.y;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = odom_quat;
            odom_broadcaster.sendTransform(odom_trans);
        }



        //里程计消息，cartographer可以使用。
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        //set the position
        odom.pose.pose.position.x = odemeterDataIn.pos.x;
        odom.pose.pose.position.y = odemeterDataIn.pos.y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation =  tf::createQuaternionMsgFromYaw(odemeterDataIn.pos.rotate);
        odom.pose.covariance={1e-3,0,0,0,0,0,0,1e-3,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e3};
//        odom.pose.covariance[0]=0.1;//{1e-3,0,0,0,0,0,0,1e-3,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e3};
//        odom.pose.covariance[7]=0.1;
//        odom.pose.covariance[35]=0.05;
//        odom.pose.covariance[14]=DBL_MAX;
//        odom.pose.covariance[21]=DBL_MAX;
//        odom.pose.covariance[28]=DBL_MAX;

        //set the velocity
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = odemeterDataIn.vel.x_move;
        odom.twist.twist.linear.y = odemeterDataIn.vel.y_move;
        odom.twist.twist.angular.z = odemeterDataIn.vel.z_rotate;
        odom.twist.covariance={1e-3,0,0,0,0,0,0,1e-3,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e3};
        //odom.twist.covariance={1e-3,0,0,0,0,0,0,1e-3,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e6,0,0,0,0,0,0,1e3};
        //publish the message
        odom_pub_->publish(odom);

    }

    return 0;
}

int RosApi::setWheelOdometerData_noRos(UserInfo userInfo, int robotIdIn, OdemeterData odemeterDataIn, int &returnFlag)
{
    encoder_odemeterData=odemeterDataIn;
    if(isUseReflectDetect)
    {
         reflectPositionMatch->setWheelOdometerData_noRos(odemeterDataIn);
    }
    if(isUseVmarkDetect)
    {
         visionPositionMatch->setWheelOdometerData_noRos(odemeterDataIn);
    }

    if(isUseAmclAdvance)
    {
        amclAdvance->setWheelOdometerData_noRos(odemeterDataIn);
    }


    returnFlag=0;
    return 1;
}

//get velocity from ROS
void RosApi::twistCmdVelCallback(const geometry_msgs::Twist &msg)
{
    rosVehicleCmdVel.x_move=msg.linear.x;
    rosVehicleCmdVel.y_move=msg.linear.y;
    rosVehicleCmdVel.z_rotate=msg.angular.z;

//    ROS_INFO("**********vehicle velocity cmd: %.2f, %.2f, %.2f",
//             rosVehicleCmdVel.x_move, rosVehicleCmdVel.y_move,
//             rosVehicleCmdVel.z_rotate);
//    UserInfo tmpUser;
//    p_powerManger->vehicleAuto(tmpUser,1,rosVehicleCmdVel.x_move,rosVehicleCmdVel.y_move,
//                               rosVehicleCmdVel.z_rotate);


}

void RosApi::frontLaserScanDataCallback(const sensor_msgs::LaserScanConstPtr &laser_scan)
{
    //出现２次原始激光数据正常，但是转发激光数据频率很慢。
    static int tmpHeartBeat=0;
    tmpHeartBeat++;
    if(1==debugFlag)
    {
        qDebug()<<"frontLaserScanDataCallback in "<<tmpHeartBeat;
    }
    if(0==laser_scan->intensities.size())
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8177, robotId );
        return ;
    }

    if(1==debugFlag)
    {
        qDebug()<<"frontLaserScanDataCallback in1"<<tmpHeartBeat;
    }
    LaserDataInfo tmpfrontOriginLaserData;
    tmpfrontOriginLaserData.angle_min=laser_scan->angle_min;
    tmpfrontOriginLaserData.angle_max=laser_scan->angle_max;
    tmpfrontOriginLaserData.angle_increment=laser_scan->angle_increment;
    tmpfrontOriginLaserData.time_increment=laser_scan->time_increment;
    tmpfrontOriginLaserData.scan_time=laser_scan->scan_time;
    tmpfrontOriginLaserData.range_min=laser_scan->range_min;
    tmpfrontOriginLaserData.range_max=laser_scan->range_max;
    tmpfrontOriginLaserData.ranges=laser_scan->ranges ;
    tmpfrontOriginLaserData.intensities=laser_scan->intensities;

    if(laserScanDevice_infoList->size()>0)
    {
        //飞点剔除
        if(1==(*laserScanDevice_infoList)[0].isEnableFlyPointDelete)
        {
           laserCalculate->deleteFlyPoint((*laserScanDevice_infoList)[0].flyPoint_allowMiniumAngle,
                   tmpfrontOriginLaserData,tmpfrontOriginLaserData);
        }
        //角度剔除
        laserCalculate->laserFilterByAngle((*laserScanDevice_infoList)[0].laserMinAngle,
                (*laserScanDevice_infoList)[0].laserMaxAngle,tmpfrontOriginLaserData);//lms1xx未知距离为２０，导致建图问题
    }

    LaserDataInfo tmpfrontFilterLaserData=tmpfrontOriginLaserData;
    switch(originalLaserFilterType)
    {
    case E_LASER_FILTER_TYPE_VMARK:
    {
        #ifdef D_USE_VISION_MATCH
         visionPositionMatch->laserFilter_test(tmpfrontOriginLaserData,tmpfrontFilterLaserData);
        #endif
         break;
    }
    case E_LASER_FILTER_TYPE_REFLECT:
    {
         reflectPositionMatch->laserFilter_test(tmpfrontOriginLaserData,tmpfrontFilterLaserData);
         break;
    }
    default:
    {
        //倍加福雷达8400点导致gmapping 崩溃，需要低于1800点。
        laserCalculate->laserFilterBySkip((*laserScanDevice_infoList)[0].skipInterval,tmpfrontFilterLaserData);
//            laserCalculate->laserFilterByIntensity(*frontOriginLaserData,*frontFilterLaserData);
    }
    }

    sensor_msgs::LaserScan tmpLaserDataToPub;
    tmpLaserDataToPub.header=laser_scan->header;
    tmpLaserDataToPub.angle_min=tmpfrontFilterLaserData.angle_min;
    tmpLaserDataToPub.angle_max=tmpfrontFilterLaserData.angle_max;
    tmpLaserDataToPub.angle_increment=tmpfrontFilterLaserData.angle_increment;
    tmpLaserDataToPub.time_increment=tmpfrontFilterLaserData.time_increment;
    tmpLaserDataToPub.scan_time=tmpfrontFilterLaserData.scan_time;
    tmpLaserDataToPub.range_min=tmpfrontFilterLaserData.range_min;
    tmpLaserDataToPub.range_max=tmpfrontFilterLaserData.range_max;
    tmpLaserDataToPub.ranges=tmpfrontFilterLaserData.ranges ;
    tmpLaserDataToPub.intensities=tmpfrontFilterLaserData.intensities;
    front_filter_laser_pub_->publish(tmpLaserDataToPub);
    if(1==debugFlag)
    {
        qDebug()<<"frontLaserScanDataCallback in3 sended"<<tmpHeartBeat;
    }

    if(isUseAmclAdvance)
    {
        sensor_msgs::LaserScan* tmpSensorData=new sensor_msgs::LaserScan;
        *tmpSensorData=tmpLaserDataToPub;
        sensor_msgs::LaserScanConstPtr tmp_laserSensorData(tmpSensorData);
        amclAdvance->setLaserSensorData(tmp_laserSensorData);
    }
    if(isUseReflectDetect)
    {
        reflectPositionMatch->setLaserSensorData(&tmpfrontOriginLaserData);
    }

    if(isUseVmarkDetect)
    {
        visionPositionMatch->setLaserSensorData(&tmpfrontOriginLaserData);
    }



    PointCloudData_3D tmpfrontOriginLaserData_pointCloud;
    PointCloudData_3D tmpfrontFilterLaserData_pointCloud;
    PointCloudData_3D tmpfrontOriginLaserData_pointCloud_translated;
    if(laserScanDevice_infoList->size()>0)
    {
        //转换点云
        laserCalculate->transformToCartesianPosition((*laserScanDevice_infoList)[0].skipInterval,
                tmpfrontOriginLaserData,tmpfrontOriginLaserData_pointCloud.point_x
                                                         ,tmpfrontOriginLaserData_pointCloud.point_y);
        int pointSize=tmpfrontOriginLaserData_pointCloud.point_x.size();
        tmpfrontOriginLaserData_pointCloud.point_z.resize(pointSize);
        for(int i=0;i<pointSize;i++)
        {
            tmpfrontOriginLaserData_pointCloud.point_z[i]=0;
        }

        laserCalculate->transformToCartesianPosition(0,tmpfrontFilterLaserData,tmpfrontFilterLaserData_pointCloud.point_x
                                                         ,tmpfrontFilterLaserData_pointCloud.point_y);
        pointSize=tmpfrontFilterLaserData_pointCloud.point_x.size();
        tmpfrontFilterLaserData_pointCloud.point_z.resize(pointSize);
        for(int i=0;i<pointSize;i++)
        {
            tmpfrontFilterLaserData_pointCloud.point_z[i]=0;
        }


        //转移到车体中心为原点
        //时间分析
        struct timespec startTime,endTime;
        clock_gettime(CLOCK_MONOTONIC, &startTime);


//        laserCalculate->tranlatePointCloutToVehicleOrigin((*laserScanDevice_infoList)[0],*frontFilterLaserData_pointCloud,
//                *frontFilterLaserData_pointCloud_translated);//todo 耗时７０ｍｓ
        laserCalculate->tranlatePointCloutToVehicleOrigin((*laserScanDevice_infoList)[0],tmpfrontOriginLaserData_pointCloud,
                tmpfrontOriginLaserData_pointCloud_translated);//todo 耗时７０ｍｓ
//        tmpfrontOriginLaserData_pointCloud_translated.timeStamp++;
        clock_gettime(CLOCK_MONOTONIC, &endTime);
        pointCloudTrans_currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
        pointCloudTrans_currentConsumed=pointCloudTrans_currentConsumed/1000.0;
        if(pointCloudTrans_currentConsumed<pointCloudTrans_minConsumed)
        {
            pointCloudTrans_minConsumed=pointCloudTrans_currentConsumed;
        }
        if(pointCloudTrans_currentConsumed>pointCloudTrans_maxConsumed)
        {
            pointCloudTrans_maxConsumed=pointCloudTrans_currentConsumed;
        }



    }

    pthread_mutex_lock(&rosDataMutex);
    *frontOriginLaserData=tmpfrontOriginLaserData;
    *frontFilterLaserData=tmpfrontFilterLaserData;
    *frontOriginLaserData_pointCloud=tmpfrontOriginLaserData_pointCloud;
    *frontFilterLaserData_pointCloud=tmpfrontFilterLaserData_pointCloud;
    tmpfrontOriginLaserData_pointCloud_translated.timeStamp=frontOriginLaserData_pointCloud_translated->timeStamp+1;
    *frontOriginLaserData_pointCloud_translated=tmpfrontOriginLaserData_pointCloud_translated;
    pthread_mutex_unlock(&rosDataMutex);

//    qDebug()<<"frontLaserScanDataCallback";
    laserDriverLoseDataCount[0]=0;//加载大地图会卡激光数据接收超时．ros::spin()正常

}

void RosApi::backLaserScanDataCallback(const sensor_msgs::LaserScanConstPtr &laser_scan)
{
    if(0==laser_scan->intensities.size())
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8178, robotId );
        return ;
    }

    LaserDataInfo tmpbackOriginLaserData;

    tmpbackOriginLaserData.angle_min=laser_scan->angle_min;
    tmpbackOriginLaserData.angle_max=laser_scan->angle_max;
    tmpbackOriginLaserData.angle_increment=laser_scan->angle_increment;
    tmpbackOriginLaserData.time_increment=laser_scan->time_increment;
    tmpbackOriginLaserData.scan_time=laser_scan->scan_time;
    tmpbackOriginLaserData.range_min=laser_scan->range_min;
    tmpbackOriginLaserData.range_max=laser_scan->range_max;
    tmpbackOriginLaserData.ranges=laser_scan->ranges ;
    tmpbackOriginLaserData.intensities=laser_scan->intensities;

//    qDebug()<<"backLaserScanDataCallback"<<tmpbackOriginLaserData.ranges.size();

    if(laserScanDevice_infoList->size()>0)
    {
        //飞点剔除
        if(1==(*laserScanDevice_infoList)[1].isEnableFlyPointDelete)
        {
           laserCalculate->deleteFlyPoint((*laserScanDevice_infoList)[1].flyPoint_allowMiniumAngle,
                   tmpbackOriginLaserData,tmpbackOriginLaserData);
        }
        //角度剔除
        laserCalculate->laserFilterByAngle((*laserScanDevice_infoList)[1].laserMinAngle,
                (*laserScanDevice_infoList)[1].laserMaxAngle,tmpbackOriginLaserData);
    }

    PointCloudData_3D tmpbackOriginLaserData_pointCloud;
    PointCloudData_3D tmpbackOriginLaserData_pointCloud_translated;
    if(laserScanDevice_infoList->size()>1)
    {
        //转换点云
        laserCalculate->transformToCartesianPosition((*laserScanDevice_infoList)[1].skipInterval,
                tmpbackOriginLaserData,tmpbackOriginLaserData_pointCloud.point_x
                                                         ,tmpbackOriginLaserData_pointCloud.point_y);

        int pointSize=tmpbackOriginLaserData_pointCloud.point_x.size();
        tmpbackOriginLaserData_pointCloud.point_z.resize(pointSize);
        for(int i=0;i<pointSize;i++)
        {
            tmpbackOriginLaserData_pointCloud.point_z[i]=0;
        }

        //转移到车体中心为原点
        laserCalculate->tranlatePointCloutToVehicleOrigin((*laserScanDevice_infoList)[1],tmpbackOriginLaserData_pointCloud,
                    tmpbackOriginLaserData_pointCloud_translated);
//        backOriginLaserData_pointCloud_translated->timeStamp++;
    }

    pthread_mutex_lock(&rosDataMutex);
    *backOriginLaserData=tmpbackOriginLaserData;
    *backOriginLaserData_pointCloud=tmpbackOriginLaserData_pointCloud;
    tmpbackOriginLaserData_pointCloud_translated.timeStamp=backOriginLaserData_pointCloud_translated->timeStamp+1;
    *backOriginLaserData_pointCloud_translated=tmpbackOriginLaserData_pointCloud_translated;
    pthread_mutex_unlock(&rosDataMutex);
    laserDriverLoseDataCount[1]=0;
}

void RosApi::front3dCameraDataCallback(const sensor_msgs::PointCloud2ConstPtr &data_pointCloudIn)
{
    pthread_mutex_lock(&rosDataMutex);
    qDebug()<<"data_pointCloudIn....";
    qDebug()<<data_pointCloudIn->height;
    qDebug()<<data_pointCloudIn->width;
//    qDebug()<<data_pointCloudIn->fields;
    qDebug()<<data_pointCloudIn->is_bigendian;
    qDebug()<<data_pointCloudIn->point_step;
    qDebug()<<data_pointCloudIn->row_step;
//    qDebug()<<data_pointCloudIn->data;
    qDebug()<<data_pointCloudIn->is_dense;
//    front_3dCameraData->point_x.resize();

//    if(threeDCameraDevice_infoList->size()>1)
//    {
//    //转移到车体中心为原点
//    laserCalculate->tranlatePointCloutToVehicleOrigin(threeDCameraDevice_infoList[0],front_3dCameraData,
//            front_3dCameraData_translated);

//    }

    pthread_mutex_unlock(&rosDataMutex);
}

void RosApi::back3dCameraDataCallback(const sensor_msgs::PointCloud2ConstPtr &data_pointCloudIn)
{
    pthread_mutex_lock(&rosDataMutex);



    pthread_mutex_unlock(&rosDataMutex);
}

//send velocity command to base control
int RosApi::getVehicleVelocityComand(UserInfo userInfo, int robotIdIn, VehicleVelocity &velocityComamnd)
{
    //  velocityComamnd=rosVehicleCmdVel;
    velocityComamnd.x_move=rosVehicleCmdVel.x_move;
    velocityComamnd.y_move=rosVehicleCmdVel.y_move;
    velocityComamnd.z_rotate=rosVehicleCmdVel.z_rotate;
    return 0;
}

int RosApi::updatePciIcpPosition()
{
    if(true==isPclIcpWork)
    {
        calculateVehiclePosition_pcl(baseMapRegion,localMapRegion);//for test

    }
}

int RosApi::updateVmarkPosition()
{

    if(false==isMainProgramInitialOk)
    {
        return 0;
    }

    #ifdef D_USE_VISION_MATCH
    if(NULL==visionPositionMatch )
    {
        return -1;
    }

    if(true==isVmarkWork)
    {
        pthread_mutex_lock(&rosDataMutex);
        visionPositionMatch->setReferenceBaselinkPosition(vehiclePositionInfo_ros.position);
        visionPositionMatch->detectReflectLine(vehiclePositionInfo_vmark);
        pthread_mutex_unlock(&rosDataMutex);
        if(vehiclePositionInfo_vmark.isUpdateSucess)
        {
            visionPositionUpdateFailedCount=0;
        }

    }
    return vehiclePositionInfo_vmark.isUpdateSucess;
    #endif
}

int RosApi::updateReflectPosition()
{
//    if(false==isMainProgramInitialOk)
//    {
//        return 0;
//    }


    if(NULL==reflectPositionMatch )
    {
        return -1;
    }


    if(true==isReflectWork)
    {
//        std::vector<double> x_out,y_out,r_out;
//        visionPositionMatch->detectVMark_laser(x_out,y_out,r_out);
//        QVector<double> x_out,y_out,r_out;
//        pthread_mutex_lock(&rosDataMutex);
        //使用原始激光数据frontOriginLaserData，不要用滤波后的frontFilterLaserData。
        reflectPositionMatch->detectReflectMark_filter(vehiclePositionInfo_reflect);
        if(isUseAmclAdvance)
        {
            amclAdvance->notifyReflectPositionInfo(vehiclePositionInfo_reflect);
        }
//        pthread_mutex_unlock(&rosDataMutex);
        clock_gettime(CLOCK_MONOTONIC, &vehiclePositionInfo_reflect.matchSucessTimeStmap);

    }

    return vehiclePositionInfo_reflect.isUpdateSucess;

}

void RosApi::timeOut_slot()
{
    //timerNotStart
//    getPositionFromROS();
//    calculateVisionMatchVehiclePosition();

}

void RosApi::startNavigation_slot()
{
    qDebug()<<"startNavigation_slot";
    UserInfo tmpUser;
    int returnFlag;
    startNavigation(tmpUser,returnFlag);
}

void RosApi::moveToPosition_slot(VehiclePosition positionIn, double velocity)
{
    qDebug()<<"RosApi::moveToPosition_slot";
    UserInfo tmpUser;
    int returnFlag;
    moveToPosition(tmpUser,positionIn,velocity,returnFlag);
}

int RosApi::checkYamlMap(string mapFileName_yaml)
{
    QString   yamlfilePath = D_MAP_PATH;
    yamlfilePath+=QString::fromStdString(mapFileName_yaml);

    //yaml是否存在
    QString pgmName;
    double mapResolutionOut,xOffsetOut,yOffsetOut,rOffsetOut;
    if(1!=readYamlFile(yamlfilePath,mapResolutionOut,xOffsetOut,yOffsetOut,rOffsetOut,pgmName))
    {
       qDebug()<<"error, no file found"<<yamlfilePath;
       addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8175, robotId,0,0,0,0,yamlfilePath );
       return -1;
    }

    QString   correctImagefilePath = D_MAP_PATH + pgmName;

    //判断yaml与pgm是否一致
    QRegExp rx("(.*).yaml");
    bool isMatch=rx.exactMatch(QString::fromStdString(mapFileName_yaml));
    if(false==isMatch )
    {
        qDebug()<<"mapyaml file name error "<<QString::fromStdString(mapFileName_yaml);
        return -1;
    }
    QString mapOriginalName1=rx.cap(1);

    QRegExp rx2("(.*).pgm");
    bool isMatch2=rx2.exactMatch(pgmName);
    if(false==isMatch2 )
    {
        qDebug()<<"pgmName file name error "<<pgmName;
        return -1;
    }
    QString mapOriginalName2=rx2.cap(1);
    if(mapOriginalName1!=mapOriginalName2)
    {
        qDebug()<<"mapOriginalName1!=mapOriginalName2";
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8174, robotId, 0,0,0,0,mapOriginalName1);
        return -1;
    }

    //判断pgm是否存在。
    QFileInfo fileInfo(correctImagefilePath);
    if(false==fileInfo.isFile())
    {
        qDebug()<<"error,pgm not exist"<<correctImagefilePath;
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8176, robotId ,0,0,0,0,correctImagefilePath );
        return -1;
    }

    return 1;

}

int RosApi::readYamlFile(QString yamlFilePath,double &mapResolutionOut,
                         double &xOffsetOut,double &yOffsetOut,double &rOffsetOut,QString &imageNameOut)
{//如果文件不存在会导致ｃoredump
    qDebug()<<"RosApi::readYamlFile"<<yamlFilePath;

    QFile file(yamlFilePath);
     if (!file.exists())
     {
         qDebug()<<"QFile file not exist!!!"<<yamlFilePath;
         return -1;
     }

    std::ifstream fin(yamlFilePath.toLatin1());
    Vec3 originVec;
    try
    {
        YAML::Parser parser(fin);

        YAML::Node doc;
        if(false==parser.GetNextDocument(doc))
        {
           qDebug()<<"error, no file found,readYamlFile"<<yamlFilePath;
           return -1;
        }
        std::string  tmpString;
        doc["image"] >> tmpString;
        //cartographer生成的是图片的绝对路径
        QString tmpFileName=QString::fromStdString(tmpString);
        if(tmpFileName.contains('/'))
        {
            int first = tmpFileName.lastIndexOf ("/"); //从后面查找"/"位置
            imageNameOut = tmpFileName.right (tmpFileName.length ()-first-1); //从右边截取
        }
        else
        {
            imageNameOut=QString::fromStdString(tmpString);
        }

        doc["resolution"] >> mapResolutionOut;


        doc["origin"] >> originVec;
    }
    catch(YAML::ParserException  errorInfo)
    {
        qDebug()<<"error, yaml格式错误"<<yamlFilePath;
        return -2;
    }

    xOffsetOut=originVec.x;
    yOffsetOut=originVec.y;
    rOffsetOut=originVec.a;

    return 1;

}

int RosApi::getMapFileSize(QString pgmFilePath, double &width, double &heighth)
{
    QString displayString;
    QFile file(pgmFilePath);
     if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
         return -1;

     QTextStream in(&file);
     QString line = in.readLine();
     QString lineSecond = in.readLine();//gmapping 创建的本行是＃开头注释，ｐｓ创建的本行是宽
     QString lineThird = in.readLine();//gmapping 创建的本行是宽和高，ｐｓ创建的本行是高
     if(lineSecond.contains('#'))
     {
         qDebug()<<"gmapping picture";


         if (true!=line.isNull())
         {
             QString firstStr=lineThird.section(" ",0,0);
             qDebug()<<firstStr;
             QString secondStr=lineThird.section(" ",1,1);
             qDebug()<<secondStr;
             width=firstStr.toInt();
             heighth=secondStr.toInt();
             return 1;

         }
         else
         {
             return -1;
         }
     }
     else
     {
         qDebug()<<"photoshop picture";
         width=lineSecond.toInt();
         heighth=lineThird.toInt();
     }


     return 1;

}

int RosApi::checkLaserDriverLoseData()
{
    static int bootCount=0;
//     qDebug()<<"bootCount"<<bootCount<<"laserDriverLoseDataCount[0]"<<laserDriverLoseDataCount[0];
    if(bootCount<1000)//加载大地图会卡激光数据接收超时．ros::spin()正常
    {
        bootCount++;
    }
    if(bootCount<900)
    {
        return 0;
    }


    //报错
    if(laserErrorTimeOutCount<laserDriverLoseDataCount[0] && laserScanDevice_infoList->size()>=1)
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8170, robotId,laserDriverLoseDataCount[0], laserErrorTimeOutCount);
        isLaserLose=true;
        return -1;
    }
    if(laserErrorTimeOutCount<laserDriverLoseDataCount[1] && laserScanDevice_infoList->size()>=2)
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8171, robotId,laserDriverLoseDataCount[1], laserErrorTimeOutCount);
        isLaserLose=true;
        return -1;
    }

    //提醒，//加载地图会导致报下面提醒。
    if(laserAllowTimeOutCount<laserDriverLoseDataCount[0] && laserScanDevice_infoList->size()>=1)
    {
        addMsg(ENUM_MSG_WARNING, "RosApi", "RosApi", 8166, robotId,laserDriverLoseDataCount[0], laserAllowTimeOutCount);
        isLaserLose=true;
        return -1;
    }
    if(laserAllowTimeOutCount<laserDriverLoseDataCount[1] && laserScanDevice_infoList->size()>=2)
    {
        addMsg(ENUM_MSG_WARNING, "RosApi", "RosApi", 8168, robotId,laserDriverLoseDataCount[1], laserAllowTimeOutCount);
        isLaserLose=true;
        return -1;
    }
    isLaserLose=false;
    return 1;
}

int RosApi::broadcastStaticTransform()
{
    ros::Duration transform_tolerance_;
    transform_tolerance_.fromSec(0.1);
    //front_laser
    if(laserScanDevice_infoList->size()>=1)
    {
        tf::Transform transform_front_laser;
        transform_front_laser.setOrigin( tf::Vector3((*laserScanDevice_infoList)[0].pose_x,
                                         (*laserScanDevice_infoList)[0].pose_y, (*laserScanDevice_infoList)[0].pose_z) );
        tf::Quaternion quat;
        quat.setRPY((*laserScanDevice_infoList)[0].pose_roll, (*laserScanDevice_infoList)[0].pose_pitch,
                                                                                (*laserScanDevice_infoList)[0].pose_yaw);
        transform_front_laser.setRotation(quat);
        tfBroadcaster->sendTransform(tf::StampedTransform(transform_front_laser,
                                                          ros::Time::now()+transform_tolerance_, "/base_link", "/front_laser"));
    }

    //back_laser
    if(laserScanDevice_infoList->size()>=2)
    {
        tf::Transform transform_back_laser;
        transform_back_laser.setOrigin( tf::Vector3((*laserScanDevice_infoList)[1].pose_x,
                                         (*laserScanDevice_infoList)[1].pose_y, (*laserScanDevice_infoList)[1].pose_z) );
        tf::Quaternion quat;
        quat.setRPY((*laserScanDevice_infoList)[1].pose_roll, (*laserScanDevice_infoList)[1].pose_pitch,
                                                                                (*laserScanDevice_infoList)[1].pose_yaw);
        transform_back_laser.setRotation(quat);
        tfBroadcaster->sendTransform(tf::StampedTransform(transform_back_laser,
                                                          ros::Time::now()+transform_tolerance_, "/base_link", "/back_laser"));
    }


    return 1;
}



int RosApi::syncEncoderOdom(VehiclePosition startOdome)
{
    //startOdome 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(0,0,startOdome.rotate);
    KDL::Vector tmpVector(startOdome.x,startOdome.y,0);
    KDL::Frame frame_startOdome(tmpRotation,tmpVector);

    (*frame_compensation_encoderOdom)=frame_origin_encoderOdom->Inverse()*frame_startOdome;

    return 1;
}

int RosApi::initalLaserDevice(std::vector<LaserScanDeviceInfo> &laserScanDevice_infoListOut)
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    tmpFilePath=tmpFilePath+D_LASER_SCAN_DEVICE_CONFIG_FILE_PATH;
    int tmpReturn;
    tmpReturn=readLaserScanConfigFile(tmpFilePath,laserScanDevice_infoListOut);
    if(1!=tmpReturn)
    {
        qDebug()<<"error, initalLaserDevice";
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8154, robotId );
        return -1;
    }
    for(int i=0;i<laserScanDevice_infoListOut.size();i++)
    {
        if(1==laserScanDevice_infoListOut[i].isInUse)
        {
            // launch laser file
            //std::string   launchCommand="roslaunch sick_tim sick_tim571_2050101.launch &";
            int tmpReturn=rosLaunchCommand(QString::fromStdString(laserScanDevice_infoListOut[i].laserLaunchCommand));
            if(1!=tmpReturn)
            {
                 printf("laser scan launch  error... \n");
            }

            // set visionMatch
            if("front_scan"==laserScanDevice_infoListOut[i].driverConfig.topicName)
            {
                #ifdef D_USE_ICP
                if(NULL!=pclMatch && true== isUsePclIcp)
                {
                    pclMatch->setLaserToVehicleInfo(laserScanDevice_infoListOut[i]);
                }

                #endif
                #ifdef D_USE_VISION_MATCH
                if(NULL!=visionPositionMatch )
                {
                    visionPositionMatch->setLaserLinkInBaseLink(laserScanDevice_infoListOut[i].pose_x,laserScanDevice_infoListOut[i].pose_y,
                                  laserScanDevice_infoListOut[i].pose_z,laserScanDevice_infoListOut[i].pose_roll,
                                  laserScanDevice_infoListOut[i].pose_pitch,laserScanDevice_infoListOut[i].pose_yaw);
                }

                #endif
                if(isUseReflectDetect)
                {
                    reflectPositionMatch->setLaserLinkInBaseLink(laserScanDevice_infoListOut[i].pose_x,laserScanDevice_infoListOut[i].pose_y,
                                  laserScanDevice_infoListOut[i].pose_z,laserScanDevice_infoListOut[i].pose_roll,
                                  laserScanDevice_infoListOut[i].pose_pitch,laserScanDevice_infoListOut[i].pose_yaw);
                }

                if(isUseAmclAdvance)
                {
                    amclAdvance->setLaserLinkInBaseLink(laserScanDevice_infoListOut[i].pose_x,laserScanDevice_infoListOut[i].pose_y,
                              laserScanDevice_infoListOut[i].pose_z,laserScanDevice_infoListOut[i].pose_roll,
                              laserScanDevice_infoListOut[i].pose_pitch,laserScanDevice_infoListOut[i].pose_yaw);
                }



            }
            #ifdef D_USE_INTERNAL_LASER_DRIVER
            //初始化内部驱动
            if(true==laserScanDevice_infoListOut[i].driverConfig.isUseInternalDriver)
            {
                LaserDriverAbstract* tmp_driver=NULL;
                switch(laserScanDevice_infoListOut[i].driverConfig.venderId)
                {
                case 1:
                {
                    if(1==laserScanDevice_infoListOut[i].driverConfig.typeId)
                    {
                        tmp_driver=new LaserDriverSickTime(laserScanDevice_infoListOut[i].driverConfig);
                        internalLaserScanDriver.push_back(tmp_driver);
                    }
                    else if(2==laserScanDevice_infoListOut[i].driverConfig.typeId)
                    {
                        tmp_driver=new LaserDriverSickLms(laserScanDevice_infoListOut[i].driverConfig);
                        internalLaserScanDriver.push_back(tmp_driver);
                    }
                    break;
                }
                case 2:
                {
//                    if(1==laserScanDevice_infoListOut[i].driverConfig.typeId)
//                    {
//                        tmp_driver=new LaserDriverBeiyang(laserScanDevice_infoListOut[i].driverConfig);
//                        internalLaserScanDriver.push_back(tmp_driver);
//                    }
                    break;
                }
                case 3:
                {

                    break;
                }
                case 4:
                {
                    if(1==laserScanDevice_infoListOut[i].driverConfig.typeId)
                    {
                        tmp_driver=new LaserDriverKeli(laserScanDevice_infoListOut[i].driverConfig);
                        internalLaserScanDriver.push_back(tmp_driver);
                    }
                    break;
                }
                }
            }
            //检查驱动是否初始化成功
            for(int i=0;i<internalLaserScanDriver.size();i++)
            {
                if(false==internalLaserScanDriver[i]->isDriverStatusOk())
                {
                    qDebug()<<"error, laser driver initial failed";
                    addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8161, robotId ,i);
                }
            }
            #endif
        }
    }

    return 1;
}

int RosApi::inital3dCameraDevice(std::vector<LaserScanDeviceInfo> &threeDCameraDevice_infoListOut)
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    tmpFilePath=tmpFilePath+D_3DCAMERA_DEVICE_CONFIG_FILE_PATH;

    int tmpReturn;
    tmpReturn=read3dCameraConfigFile(tmpFilePath,threeDCameraDevice_infoListOut);
    if(1!=tmpReturn)
    {
        qDebug()<<"error, initalLaserDevice";
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8155, robotId );
        return -1;
    }

    for(int i=0;i<threeDCameraDevice_infoListOut.size();i++)
    {
        if(1==threeDCameraDevice_infoListOut[i].isInUse)
        {
            int tmpReturn=rosLaunchCommand(QString::fromStdString(threeDCameraDevice_infoListOut[i].laserLaunchCommand));
            if(1!=tmpReturn)
            {
                printf("3D CAMERA launch  error... \n");
            }


        }
    }

    return 1;
}

int RosApi::loadMapFromYaml(QString yamlFilePath)
{
    // parse yaml file
    double mapResolution;
    QString imageFilePath;
    std::ifstream fin(yamlFilePath.toLatin1());
    YAML::Parser parser(fin);
    YAML::Node doc;
    if(false==parser.GetNextDocument(doc))
    {
       qDebug()<<"error, no file found";
       return -1;
    }
    imageFilePath.clear();
    imageFilePath=D_MAP_PATH;
    std::string  tmpString;
    doc["image"] >> tmpString;
    imageFilePath+=QString::fromStdString(tmpString);
    qDebug()<<imageFilePath;
    doc["resolution"] >> mapResolution;
    Vec3 originVec;
    doc["origin"] >> originVec;

    //visionMatch load
    #ifdef D_USE_ICP
    if(NULL!=pclMatch && true== isUsePclIcp)
    {
        pclMatch->loadBaseMap(imageFilePath,mapResolution,originVec.x,originVec.y,originVec.a);
    }
    #endif
    #ifdef D_USE_VISION_MATCH
    if(NULL!=visionPositionMatch)
    {
//        visionPositionMatch->loadBaseMap(imageFilePath,mapResolution,originVec.x,originVec.y,originVec.a);
    }
    #endif

    return 1;

}

int RosApi::getFrontFilterLaserData(LaserDataInfo &frontFilterLaserDataOut)
{
    pthread_mutex_lock(&rosDataMutex);
    frontFilterLaserDataOut=*frontFilterLaserData;
    pthread_mutex_unlock(&rosDataMutex);
}

int RosApi::addVitrualBlockFromMapFile(QString mapFilePath)
{
   int index = mapFilePath.lastIndexOf ("."); //从后面查找"."位置
   QString blockFilePath = mapFilePath.left (index); //
   blockFilePath+=".xml";
   qDebug()<<blockFilePath;
   return addVitrualBlock(myUserInfo,blockFilePath.toStdString(),myReturnKey);
}

int RosApi::readConfigFile(QString configFilePathIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, loadDefaultNavagationParameter open file error！";
       return -1;
    }

    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "CommSetting", ok );
    if( !ok )
    {

        qDebug()<<"error, loadDefaultNavagationParameter node error！";
        return -2;
    }

    //节点ｒｅａｄ数据
    int tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "mapSolver", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -3;
    }
    mapSolver=(E_MAP_SOLVER)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "defaultMapFileName", ok), currentMapFileName  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -4;
    }
    qDebug()<<"currentMapFileName="<<currentMapFileName;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "carBorderFileName", ok), carBorderFileName  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -5;
    }
    qDebug()<<"carBorderFileName="<<carBorderFileName;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomSolver", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -6;
    }
    odomSolver=(E_ODOM_SOLVER)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "locationSolver", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -7;
    }
    locationSolver=(E_LOCATION_SOLVER)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "trajectoryGlobalSolver", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -8;
    }
    trajectoryGlobalSolver=(E_TRAJECTORY_GLOBAL_SOLVER)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "trajectoryLocalSolver", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -9;
    }
    trajectoryLocalSolver=(E_TRAJECTORY_LOCAL_SOLVER)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "localPlanXRange", ok), localPlanXRange  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -10;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "localPlanYRange", ok), localPlanYRange  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -11;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "localBarrierOffset", ok), localBarrierOffset  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -12;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "globalBarrierOffset", ok), globalBarrierOffset  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -13;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "replanCountLimit", ok), replanCountLimit  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -14;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "arrivedAccuracy_linear", ok),
                       arrivedAccuracy_linear  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -15;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "arrivedAccuracy_angular", ok),
                       arrivedAccuracy_angular  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -16;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "defaultVelocity", ok), currentSettingVelocity  );
    if( !ok )
    {
        qDebug()<<"error, setDefaultMapName node error！";
        return -17;
    }
    int tmpFlag;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseAmclAdvance", ok), tmpFlag  );
    if( !ok )
    {
        qDebug()<<"error, isUseAmclAdvance node error！";
        return -18;
    }
    isUseAmclAdvance=tmpFlag;

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseVmarkDetect", ok), isUseVmarkDetect  );
    if( !ok )
    {
        qDebug()<<"error, isUseVmarkDetect node error！";
        return -19;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseReflectDetect", ok), isUseReflectDetect  );
    if( !ok )
    {
        qDebug()<<"error, isUseReflectDetect node error！";
        return -20;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseSickTimDriver", ok), tmpFlag  );
    if( !ok )
    {
        qDebug()<<"error, isUseSickTimDriver node error！";
        return -21;
    }
    isUseSickTimDriver=tmpFlag;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseLaserOdometry", ok), tmpFlag  );
    if( !ok )
    {
        qDebug()<<"error, isUseLaserOdometry node error！";
        return -22;
    }
    isUseLaserOdometry=tmpFlag;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUsePclIcp", ok), tmpFlag  );
    if( !ok )
    {
        qDebug()<<"error, isUsePclIcp node error！";
        return -23;
    }
    isUsePclIcp=tmpFlag;

    xmlParser.readXml( xmlParser.findSubNode(inNode, "amclMaxDogCount", ok), amclMaxDogCount  );
    if( !ok )
    {
        qDebug()<<"error, amclMaxDogCount node error！";
        return -24;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "amclInitialPoseCalculateCount", ok), amclInitialPoseCalculateCount  );
    if( !ok )
    {
        qDebug()<<"error, amclInitialPoseCalculateCount node error！";
        return -24;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "rosTimer", ok), rosTimer  );
    if( !ok )
    {
        qDebug()<<"error, rosTimer node error！";
        return -25;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "isUseRosLaserMsg", ok), isUseRosLaserMsg  );
    if( !ok )
    {
        qDebug()<<"error, isUseRosLaserMsg node error！";
        return -26;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "laserAllowTimeOutCount", ok), laserAllowTimeOutCount  );
    if( !ok )
    {
        qDebug()<<"error, laserAllowTimeOutCount node error！";
        return -27;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "laserErrorTimeOutCount", ok), laserErrorTimeOutCount  );
    if( !ok )
    {
        qDebug()<<"error, laserErrorTimeOutCount node error！";
        return -28;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "vmarkCalculateSkip", ok), vmarkCalculateSkip  );
    if( !ok )
    {
        qDebug()<<"error, vmarkCalculateSkip node error！";
        return -29;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "reflectCalculateSkip", ok), reflectCalculateSkip  );
    if( !ok )
    {
        qDebug()<<"error, reflectCalculateSkip node error！";
        return -30;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "setPosition_cov_xy", ok), setPosition_cov_xy  );
    if( !ok )
    {
        qDebug()<<"error, setPosition_cov_xy node error！";
        return -30;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "setPosition_cov_r", ok), setPosition_cov_r  );
    if( !ok )
    {
        qDebug()<<"error, setPosition_cov_r node error！";
        return -30;
    }

    //-------------read pclicp----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "PclIcp", ok );
    if( !ok )
    {

        qDebug()<<"error, loadDefaultNavagationParameter node error！";
        return -31;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "method", ok), tmpData  );
    if( !ok )
    {
        qDebug()<<"error, method node error！";
        return -32;
    }
    pclCalculationPara.method=(E_PCL_ICP_METHOD)tmpData;
    xmlParser.readXml( xmlParser.findSubNode(inNode, "maximumIterations", ok), pclCalculationPara.maximumIterations  );
    if( !ok )
    {
        qDebug()<<"error, maximumIterations node error！";
        return -33;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "transformationEpsilon", ok), pclCalculationPara.transformationEpsilon  );
    if( !ok )
    {
        qDebug()<<"error, transformationEpsilon node error！";
        return -34;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "euclideanFitnessEpsilon", ok), pclCalculationPara.euclideanFitnessEpsilon  );
    if( !ok )
    {
        qDebug()<<"error, euclideanFitnessEpsilon node error！";
        return -35;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "maxCorrespondenceDistance", ok), pclCalculationPara.maxCorrespondenceDistance  );
    if( !ok )
    {
        qDebug()<<"error, maxCorrespondenceDistance node error！";
        return -36;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "maxCalculateTime", ok), pclCalculationPara.maxCalculateTime  );
    if( !ok )
    {
        qDebug()<<"error, maxCalculateTime node error！";
        return -37;
    }

    //-------------read laserOdom----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "laserOdom", ok );
    if( !ok )
    {

        qDebug()<<"error, loadDefaultNavagationParameter node error！";
        return -38;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "skipFrameCount", ok), skipFrameCount  );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -39;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "skipLaserPointCount", ok), skipLaserPointCount  );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -40;
    }


    //-------------read RosLaunch----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "RosLaunch", ok );
    ros_launch_command.clear();
    QString tmpCommand;
    QDomNodeList devNodes = inNode.childNodes();
    QDomNode devND;
    for(int i=0;i<devNodes.size();i++)
    {
        devND =  devNodes.at(i);
        xmlParser.readXml( devND, tmpCommand );
        ros_launch_command.append(tmpCommand);

    }

    xmlParser.closeXml( );

    return 1;
}

int RosApi::loadDefaultNavagationParameter(QString configFilePath)
{
    if(1!=readConfigFile(configFilePath))
    {
        qDebug()<<"error, loadDefaultNavagationParameter！";
    }

#ifdef D_USE_ICP
    if(NULL!=pclMatch && true== isUsePclIcp)
    {
        pclMatch->setMatchCalculateParameter(pclCalculationPara);
    }
#endif
    setOdomSolver(myUserInfo,odomSolver,myReturnKey);
    setLocationSolver(myUserInfo,locationSolver,myReturnKey);
    setTrajectoryGlobalPlanSolver(myUserInfo,trajectoryGlobalSolver,myReturnKey);
    setTrajectoryLocalPlanSolver(myUserInfo,trajectoryLocalSolver,myReturnKey);
    setReplanCoutLimitWhenFailure(myUserInfo,replanCountLimit,myReturnKey);
    startNavigation(myUserInfo,myReturnKey);
    setPositionArrivedAccuracy(myUserInfo,arrivedAccuracy_linear,arrivedAccuracy_angular,myReturnKey);
    setLocalPlanRange(myUserInfo,localPlanXRange,localPlanYRange,myReturnKey);
    //setVehicleBorder(carBorderFileName);没有完整文件路径
    setLocalBarrierOffset(myUserInfo,localBarrierOffset,myReturnKey);
    setGlobalBarrierOffset(myUserInfo,globalBarrierOffset,myReturnKey);
    loadMap(myUserInfo,currentMapFileName.toStdString(),myReturnKey);
    setMapSolver(myUserInfo,mapSolver,1,myReturnKey);
    return 1;

}

//calculate the lenght of global path
void RosApi::gloablPathLenghtCallback(const nav_msgs::Path& path)
{

    int len=sizeof(path.poses)/sizeof(path.poses[0]);
    path_lenght=0;
    for(int i=0;i<len;i++)
    {
        path_lenght+=sqrt(pow((path.poses[i+1].pose.position.x - path.poses[i].pose.position.x),2) +
                pow((path.poses[i+1].pose.position.y - path.poses[i].pose.position.y), 2));
    }
}

//ROS spin
void RosApi::rosLoop()
{

}



void RosApi::killFunction()
{
    printf(" type 'c' + enter to kill the program!!!");
    char key[20];
    scanf("%s",key);
    if('c'==key[0])
    {
       exit(EXIT_SUCCESS);
    }
    else
    {
        printf(" your type key is not 'c', but %s \n",key);
    }
}

//subscribe to goal topic
void RosApi::movebaseGoalCallback(const move_base_msgs::MoveBaseActionGoal& goal)
{
    *current_goal=goal.goal;
}


//int RosApi::getPositionFromROS()
//{
//    int returnResult;
//    vector<string> Vstring;
//    ros::master::getNodes(Vstring);


//    tf::StampedTransform transform;
//    bool tf_ok=true;

//    if(((find(Vstring.begin(),Vstring.end(),"/move_base"))!=Vstring.end())&&
//            ((find(Vstring.begin(),Vstring.end(),"/amcl"))!=Vstring.end()))
//    {
//        try
//        {
////            listener.waitForTransform("/map", "/base_link", ros::Time::now(), ros::Duration(0.1));
//            listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
//        }
//        catch (tf::TransformException ex)
//        {
//            ROS_ERROR("%s", ex.what());
////            ros::Duration(1.0).sleep();
//            tf_ok=false;
//        }
//        if(tf_ok)
//        {

//            vehiclePositionInfo_ros.position.x =transform.getOrigin().getX();
//            vehiclePositionInfo_ros.position.y =transform.getOrigin().getY();
//            tf::Quaternion q=transform.getRotation();
//            vehiclePositionInfo_ros.position.rotate=tf::getYaw(q);
//            clock_gettime(CLOCK_MONOTONIC, &vehiclePositionInfo_ros.matchSucessTimeStmap);
//            vehiclePositionInfo_ros.isUpdateSucess=true;
////            printf("positionReturn: %f %f %f \n",rosVehiclePosition.x,rosVehiclePosition.y,rosVehiclePosition.rotate);
//            returnResult=1;
//        }
//        else
//        {
//            returnResult=0;
//        }
//    }
//    else
//    {
////        printf("/move_base has not start yet \n");
//        return 0;
//    }
//    return 1;
//}

int RosApi::getVehiclePositionFrom_tf(PositionInfo &positionInfoOut)
{
    static int bootCount=0;
    if(bootCount<1000)
    {
        bootCount++;
    }

    static int waitTFCount=0;
    if(false==isMainProgramInitialOk || waitTFCount <60)
    {
        waitTFCount++;
        return 0;
    }



    int returnResult;
//    vector<string> Vstring;
//    ros::master::getNodes(Vstring);

    tf::StampedTransform transform;
    bool tf_ok=true;
    try
    {
        //如果ａｍｃｌ没有启动，会报错找不到frame
//        listener->waitForTransform("/map", "/base_link", ros::Time::now(), ros::Duration(1));//最多等待1s
        listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);//可能是时间不准确，导致多个周期都没有变。
    }
    catch (tf::TransformException ex)
    {
        printf("tf error,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,%s", ex.what());
        qDebug()<<"2 tf error,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,";
        // ros::Duration(1.0).sleep();
        tf_ok=false;
    }
    isTfOk=tf_ok;
    static int tfErrorCount=0;
    if(tf_ok)
    {

        positionInfoOut.maxMatchError=0;
        positionInfoOut.position.x =transform.getOrigin().getX();
        positionInfoOut.position.y =transform.getOrigin().getY();
        tf::Quaternion q=transform.getRotation();
        positionInfoOut.position.rotate=tf::getYaw(q);
        clock_gettime(CLOCK_MONOTONIC, &positionInfoOut.matchSucessTimeStmap);
        positionInfoOut.isUpdateSucess=true;
        //printf("positionReturn: %f %f %f \n",rosVehiclePosition.x,rosVehiclePosition.y,rosVehiclePosition.rotate);
        returnResult=1;
        tfErrorCount=0;

    }
    else
    {
        positionInfoOut.isUpdateSucess=false;
        returnResult=0;
        tfErrorCount++;
    }
    vehiclePositionInfo_ros=positionInfoOut;
    if(10<tfErrorCount)
    {
        if(500<bootCount)
        {
            addMsg(ENUM_MSG_WARNING, "RosApi", "RosApi", 8167, robotId );
        }
    }


    return 1;
}

int RosApi::getVehiclePositionFrom_amcl(PositionInfo &positionInfoOut)
{
    static int bootCount=0;
    if(bootCount<1000)
    {
        bootCount++;
    }

    static int waitTFCount=0;
    if(false==isMainProgramInitialOk || waitTFCount <60)
    {
        waitTFCount++;
        return 0;
    }



    #ifdef D_USE_AMCL_ADVANCE
    if(false==isUseAmclAdvance)
    {
        return 0;
    }

    int tmpKey= amclAdvance->getVehiclePositionFrom_amcl(positionInfoOut);
    vehiclePositionInfo_ros=positionInfoOut;

    if(isReflectWork)
    {
        reflectPositionMatch->notifyAmclPositionInfo(positionInfoOut);
    }

    #endif

    return tmpKey;
}

int RosApi::getVehiclePositionFrom_ros(PositionInfo &positionInfoOut)
{
    switch(locationSolver)
    {
    case E_LOCATION_SOLVER_AMCL:
    {
        return getVehiclePositionFrom_amcl(positionInfoOut);
    }
    case E_LOCATION_SOLVER_CARTOGRAPHER_LASER:
    {
        return getVehiclePositionFrom_tf(positionInfoOut);
    }
    default:
    {
        qDebug()<<"error,unsupport location solver"<<locationSolver;
        return -1;
    }
    }
    return 0;
}

int RosApi::calculateVehiclePosition_pcl(MapRegion baseRegionIn, MapRegion localRegionIn)
{
    if(false==isMainProgramInitialOk)
    {
        return 0;
    }

    #ifdef D_USE_ICP
    if(NULL==pclMatch || false== isUsePclIcp)
    {
        return -1;
    }
//    LaserDataInfo tmpFrontFilterLaserData;
//    getFrontFilterLaserData(tmpFrontFilterLaserData);
//    std::vector<double> pointX;
//    std::vector<double> pointY;
//    laserCalculate->transformToCartesianPosition(tmpFrontFilterLaserData,pointX,pointY);
    pthread_mutex_lock(&rosDataMutex);
    pclMatch->loadLocalMap(frontFilterLaserData_pointCloud->point_x,frontFilterLaserData_pointCloud->point_y);
    pthread_mutex_unlock(&rosDataMutex);
    int tmpReturn;

    tmpReturn=pclMatch->setBaseMapRegion_m(baseRegionIn.originX,baseRegionIn.originY,
                                              baseRegionIn.width,baseRegionIn.height);
    if(1!=tmpReturn)
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8150, robotId );
        return -1;
    }
    tmpReturn=pclMatch->setLocalMapRegion_m(localRegionIn.originX,localRegionIn.originY,
                                               localRegionIn.width,localRegionIn.height);
    if(1!=tmpReturn)
    {
//        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8151, robotId );
        return -2;
    }
    double tmpPos_x,tmpPos_y,tmpPos_r,tmpMaxError,tmpScore;
    int tmpIsSucess;
    pclMatch->calculateMatch(tmpPos_x,tmpPos_y,tmpPos_r,tmpIsSucess,tmpMaxError,tmpScore);
    if(true==tmpIsSucess)
    {

        vehiclePositionInfo_pclicp.position.x =tmpPos_x;
        vehiclePositionInfo_pclicp.position.y =tmpPos_y;
        vehiclePositionInfo_pclicp.position.rotate=tmpPos_r;
        clock_gettime(CLOCK_MONOTONIC, &vehiclePositionInfo_pclicp.matchSucessTimeStmap);
        vehiclePositionInfo_pclicp.matchScore=tmpScore;
        vehiclePositionInfo_pclicp.maxMatchError=tmpMaxError;
        vehiclePositionInfo_pclicp.isUpdateSucess=true;
    }
    else
    {
        vehiclePositionInfo_pclicp.isUpdateSucess=false;
    }


    return tmpIsSucess;
#endif
}
#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* RosApi::threadRun(void *)
{
    printf("RosGuiApi theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 0;
    sched_setscheduler(0,SCHED_OTHER,&param);
    qDebug()<<"************************RosApi::threadRun pid="<<gettid();
    usleep(130000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
//    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
//    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    // 等待急停　di　模块初始化
    while(false==isShutDown)
    {
//        pthread_testcancel();/*the thread can be killed only here*/
        sleep(1);

        //等待ｅthercat初始化完毕后启动ａｍｃｌ
        #ifdef D_USE_AMCL_ADVANCE
        static bool initialAmclOk=false;
        if(false==initialAmclOk)
        {
            if(true==MessageLog::getInstance()->isDeviceMasterInitialOk())
            {
                if(true==isUseAmclAdvance)
                {
                    amclAdvance->setWorkStatus(true);
                }
                UserInfo myUserInfo;
                int myReturnKey;
                loadMap(myUserInfo,currentMapFileName.toStdString(),myReturnKey);
                initialAmclOk=true;
                isMainProgramInitialOk=true;
            }
        }
        #endif

        //exit main
        if(true==isShutDown)
        {
            killFunction();
        }

//        //周期定时器
//        static struct timespec cycleTimespec;//周期时间
//        clock_gettime(CLOCK_MONOTONIC , &cycleTimespec);//

//        while(false==isShutDown)
//        {
//            while(1)//为什么要放到这一个线程才可以使amcl工作
//            {
//                if(true==isShutDown)
//                {
//                    killFunction();
//                }
//                updateVehiclePositionOnce();
//    //            usleep(rosTimer*1000);

//                //周期定时器
//                cycleTimespec.tv_nsec += rosTimer*1000000;
//                while (cycleTimespec.tv_nsec >= NSEC_PER_SEC)
//                {
//                    cycleTimespec.tv_nsec -= NSEC_PER_SEC;
//                    cycleTimespec.tv_sec++;
//                }
//                clock_nanosleep(CLOCK_MONOTONIC , TIMER_ABSTIME, &cycleTimespec, NULL);
//            }
    //        pthread_testcancel();/*the thread can be killed only here*/
    //        usleep(100000);
    //        getPositionFromROS();
    //        updatePciIcpPosition();
    //        updatePclIcpDebugWindow();
    //        printf("RosGuiApi sleep zzzzzzzzzzzzzz\n");

//        }

    }
    printf("RosGuiApi thread quit!!!");
}

void* RosApi::threadTask( void* classPtr )
{
    printf("RosGuiApi thread run!!!");
//    qDebug()<<"RosGuiApi thread run-------------------------";
    return ((RosApi*)classPtr)->threadRun(NULL);
}

void RosApi::createThread()
{
    int ret=pthread_create( &threadId, NULL, &RosApi::threadTask,this);
    printf("RosGuiApi createThread!!!");
    //    qDebug()<<"RosGuiApi createThread-------------------------";
}

#define NSEC_PER_SEC 1000000000
void* RosApi::threadRun2(void *)
{
    printf("RosGuiApi theread2 start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 0;
    sched_setscheduler(50,SCHED_FIFO,&param);
    qDebug()<<"************************RosApi::threadRun2 pid="<<gettid();
    usleep(379000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
//    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
//    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    //周期定时器
    static struct timespec cycleTimespec;//周期时间
    clock_gettime(CLOCK_MONOTONIC , &cycleTimespec);//

    while(false==isShutDown)
    {
        while(1)//为什么要放到这一个线程才可以使amcl工作
        {
            //时间分析
            struct timespec startTime,endTime;
            clock_gettime(CLOCK_MONOTONIC, &startTime);

            if(true==isShutDown)
            {
                killFunction();
            }
            updateVehiclePositionOnce();

            clock_gettime(CLOCK_MONOTONIC, &endTime);
            rosThread_currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
            rosThread_currentConsumed=rosThread_currentConsumed/1000.0;
            if(rosThread_currentConsumed<rosThread_minConsumed)
            {
                rosThread_minConsumed=rosThread_currentConsumed;
            }
            if(rosThread_currentConsumed>rosThread_maxConsumed)
            {
                rosThread_maxConsumed=rosThread_currentConsumed;
            }

//            usleep(rosTimer*1000);

            //周期定时器
            cycleTimespec.tv_nsec += rosTimer*1000000;
            while (cycleTimespec.tv_nsec >= NSEC_PER_SEC)
            {
                cycleTimespec.tv_nsec -= NSEC_PER_SEC;
                cycleTimespec.tv_sec++;
            }
            clock_nanosleep(CLOCK_MONOTONIC , TIMER_ABSTIME, &cycleTimespec, NULL);
        }
//        pthread_testcancel();/*the thread can be killed only here*/
//        usleep(100000);
//        getPositionFromROS();
//        updatePciIcpPosition();
//        updatePclIcpDebugWindow();
//        printf("RosGuiApi sleep2 zzzzzzzzzzzzzz\n");

    }
    printf("RosGuiApi thread2 quit!!!");
}

void* RosApi::threadTask2( void* classPtr )
{
    printf("RosGuiApi thread2 run!!!");
//    qDebug()<<"RosGuiApi thread run-------------------------";
    return ((RosApi*)classPtr)->threadRun2(NULL);
}

void RosApi::createThread2()
{
    int ret=pthread_create( &threadId2, NULL, &RosApi::threadTask2,this);
    printf("RosGuiApi createThread2!!!");
    //    qDebug()<<"RosGuiApi createThread2-------------------------";
}


int RosApi::saveCarBorder(std::vector<double> point_x, std::vector<double> point_y)
{

}

int RosApi::readLaserScanConfigFile(QString configFilePathIn, std::vector<LaserScanDeviceInfo> &infoListOut)
{
    infoListOut.clear();
    LaserScanDeviceInfo tmpDeviceInfo;
    DomParser domparser;
    bool ok=domparser.openXml( configFilePathIn, QFile::ReadOnly | QFile::Text );
    if(!ok)
    {
        qDebug()<<"readLaserScanConfigFile error "<<ok;
        return -1;
    }

    QDomNodeList devNodes = domparser.getRootDomElement().childNodes();

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);

//        domparser.readXml( domparser.findSubNode( devND, "laserScanName", ok ), tmpDeviceInfo.laserScanName );
//        if(!ok)
//        {
//           return -100-1-i;
//        }
//        domparser.readXml( domparser.findSubNode( devND, "vendorId", ok ), tmpDeviceInfo.vendorId );
//        if(!ok)
//        {
//           return -100-2-i;
//        }
        domparser.readXml( domparser.findSubNode( devND, "laserLaunchCommand", ok ), tmpDeviceInfo.laserLaunchCommand );
        if(!ok)
        {
           return -100-3-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_x", ok ), tmpDeviceInfo.pose_x );
        if(!ok)
        {
           return -100-4-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_y", ok ), tmpDeviceInfo.pose_y );
        if(!ok)
        {
           return -100-5-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_z", ok ), tmpDeviceInfo.pose_z );
        if(!ok)
        {
           return -100-6-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_yaw", ok ), tmpDeviceInfo.pose_yaw );
        if(!ok)
        {
           return -100-7-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_pitch", ok ), tmpDeviceInfo.pose_pitch );
        if(!ok)
        {
           return -100-8-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_roll", ok ), tmpDeviceInfo.pose_roll );
        if(!ok)
        {
           return -100-9-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "isInUse", ok ), tmpDeviceInfo.isInUse );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "skipInterval", ok ), tmpDeviceInfo.skipInterval );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "isEnableFlyPointDelete", ok ), tmpDeviceInfo.isEnableFlyPointDelete );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "flyPoint_allowMiniumAngle", ok ), tmpDeviceInfo.flyPoint_allowMiniumAngle );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "laserMinAngle", ok ), tmpDeviceInfo.laserMinAngle );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "laserMaxAngle", ok ), tmpDeviceInfo.laserMaxAngle );
        if(!ok)
        {
           return -100-10-i;
        }

        //读驱动信息
        int tmpBool;
        QDomNode driverND=domparser.findSubNode( devND, "driverSetting", ok );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "topicName", ok ),
                           tmpDeviceInfo.driverConfig.topicName );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "isUseInternalDriver", ok ),tmpBool);
        tmpDeviceInfo.driverConfig.isUseInternalDriver=tmpBool;
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "venderId", ok ),
                           tmpDeviceInfo.driverConfig.venderId );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "typeId", ok ),
                           tmpDeviceInfo.driverConfig.typeId );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "min_ang", ok ),
                           tmpDeviceInfo.driverConfig.min_ang );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "max_ang", ok ),
                           tmpDeviceInfo.driverConfig.max_ang );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "range_min", ok ),
                           tmpDeviceInfo.driverConfig.range_min );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "range_max", ok ),
                           tmpDeviceInfo.driverConfig.range_max );
        if(!ok)
        {
           return -100-10-i;
        }

        domparser.readXml( domparser.findSubNode( driverND, "intensity", ok ),tmpBool);
        tmpDeviceInfo.driverConfig.intensity =tmpBool;
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "skip", ok ),
                           tmpDeviceInfo.driverConfig.skip );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "frame_id", ok ),
                           tmpDeviceInfo.driverConfig.frame_id );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "time_offset", ok ),
                           tmpDeviceInfo.driverConfig.time_offset );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "publish_datagram", ok ),tmpBool);
        tmpDeviceInfo.driverConfig.publish_datagram =tmpBool;
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "subscribe_datagram", ok ),tmpBool );
        tmpDeviceInfo.driverConfig.subscribe_datagram =tmpBool;
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "device_number", ok ),
                           tmpDeviceInfo.driverConfig.device_number );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "hostname", ok ),
                           tmpDeviceInfo.driverConfig.hostname );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "port", ok ),
                           tmpDeviceInfo.driverConfig.port );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "timelimit", ok ),
                           tmpDeviceInfo.driverConfig.timelimit );
        if(!ok)
        {
           return -100-10-i;
        }

        tmpDeviceInfo.laserScanName=tmpDeviceInfo.driverConfig.topicName;


        infoListOut.push_back(tmpDeviceInfo);//华磊，为什么会导致ｃｏｒｅdump

    }

    domparser.closeXml();

    return 1;

}

int RosApi::read3dCameraConfigFile(QString configFilePathIn, std::vector<LaserScanDeviceInfo> &infoListOut)
{
    infoListOut.clear();
    LaserScanDeviceInfo tmpDeviceInfo;
    DomParser domparser;
    bool ok=domparser.openXml( configFilePathIn, QFile::ReadOnly | QFile::Text );
    if(!ok)
    {
        qDebug()<<"read3dCameraConfigFile error "<<ok;
        return -1;
    }

    QDomNodeList devNodes = domparser.getRootDomElement().childNodes();

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);

//        domparser.readXml( domparser.findSubNode( devND, "CameraName", ok ), tmpDeviceInfo.laserScanName );
//        if(!ok)
//        {
//           return -100-1-i;
//        }
//        domparser.readXml( domparser.findSubNode( devND, "vendorId", ok ), tmpDeviceInfo.vendorId );
//        if(!ok)
//        {
//           return -100-2-i;
//        }
        domparser.readXml( domparser.findSubNode( devND, "laserLaunchCommand", ok ), tmpDeviceInfo.laserLaunchCommand );
        if(!ok)
        {
           return -100-3-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_x", ok ), tmpDeviceInfo.pose_x );
        if(!ok)
        {
           return -100-4-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_y", ok ), tmpDeviceInfo.pose_y );
        if(!ok)
        {
           return -100-5-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_z", ok ), tmpDeviceInfo.pose_z );
        if(!ok)
        {
           return -100-6-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_yaw", ok ), tmpDeviceInfo.pose_yaw );
        if(!ok)
        {
           return -100-7-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_pitch", ok ), tmpDeviceInfo.pose_pitch );
        if(!ok)
        {
           return -100-8-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "pose_roll", ok ), tmpDeviceInfo.pose_roll );
        if(!ok)
        {
           return -100-9-i;
        }
        domparser.readXml( domparser.findSubNode( devND, "isInUse", ok ), tmpDeviceInfo.isInUse );
        if(!ok)
        {
           return -100-10-i;
        }

        //读驱动信息
        int tmpBool;
        QDomNode driverND=domparser.findSubNode( devND, "driverSetting", ok );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "topicName", ok ),
                           tmpDeviceInfo.driverConfig.topicName );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "isUseInternalDriver", ok ),tmpBool);
        tmpDeviceInfo.driverConfig.isUseInternalDriver=tmpBool;
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "venderId", ok ),
                           tmpDeviceInfo.driverConfig.venderId );
        if(!ok)
        {
           return -100-10-i;
        }
        domparser.readXml( domparser.findSubNode( driverND, "typeId", ok ),
                           tmpDeviceInfo.driverConfig.typeId );
        if(!ok)
        {
           return -100-10-i;
        }

        infoListOut.push_back(tmpDeviceInfo);

    }

    domparser.closeXml();

    return 1;
}

//int RosApi::initialSickTimDriver()
//{
//#ifdef D_USE_SICK_TIM_DRIVER
//    if(false==isUseSickTimDriver)
//    {
//        return -1;
//    }

//    ros::NodeHandle nhPriv("~");
//    bool useTCP = true;
//    std::string hostname="192.168.2.80";
////    if(nhPriv.getParam("hostname", hostname,"192.168.2.80")) {
////        useTCP = true;
////    }
//    std::string port;
//    nhPriv.param<std::string>("port", port, "2112");

//    int timelimit;
//    nhPriv.param("timelimit", timelimit, 5);

//    bool subscribe_datagram;
//    int device_number;
//    nhPriv.param("subscribe_datagram", subscribe_datagram, false);
//    nhPriv.param("device_number", device_number, 0);

//    parser = new sick_tim::SickTim5512050001Parser();

//    double param;
//    parser->set_range_min(0.05);
//    parser->set_range_max(10.0);
//    parser->set_time_increment(0.000061722);
////    if (nhPriv.getParam("range_min", param))
////    {
////      parser->set_range_min(param);
////    }
////    if (nhPriv.getParam("range_max", param,10.0))
////    {
////      parser->set_range_max(param);
////    }
////    if (nhPriv.getParam("time_increment", param,0.000061722))
////    {
////      parser->set_time_increment(param);
////    }


//    int result = sick_tim::ExitError;

//    int tryCount=0;
//    while(tryCount<7)
//    {
//        // Atempt to connect/reconnect
//        if (subscribe_datagram)
//          sickTimDriver = new sick_tim::SickTimCommonMockup(parser);
//        else if (useTCP)
//          sickTimDriver = new sick_tim::SickTimCommonTcp(hostname, port, timelimit, parser,"front_scan");//话题名称
//        else
//          sickTimDriver = new sick_tim::SickTimCommonUsb(parser, device_number);
//        result = sickTimDriver->init();
//        LaserDriverConfig configIn;
//        configIn.frame_id="front_laser";//激光tf基座名称
//        update_laser_driver_config(configIn);
//        if(result == sick_tim::ExitSuccess)
//        {
//            break;
//        }
//        else
//        {
//            delete sickTimDriver;
//            sickTimDriver=NULL;

//            if (result == sick_tim::ExitFatal)
//              return result;

//            if (ros::ok() && !subscribe_datagram && !useTCP)
//              ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
//        }
//        tryCount++;

//    }
//    if(result != sick_tim::ExitSuccess)
//    {
//        qDebug()<<"error, canbus updateDomainData error!";
//        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi" , 8153, -1);
//        return result;
//    }
//    return result;
//#endif

//}

//int RosApi::shutdownSickTimDriver()
//{
//#ifdef D_USE_SICK_TIM_DRIVER
//    qDebug()<<"shutdownSickTimDriver";
//    if(false==isUseSickTimDriver)
//    {
//        return -1;
//    }
//    if(NULL!=sickTimDriver)
//    {
//        delete sickTimDriver;
//        delete parser;
//    }
//    return 1;
//#endif
//}

//int RosApi::sickTimDriverLoopOnce()
//{
//#ifdef D_USE_SICK_TIM_DRIVER
//    if(false==isUseSickTimDriver)
//    {
//        return -1;
//    }
//    int result = sick_tim::ExitError;
//    if(NULL!=sickTimDriver)
//    {
//        result =sickTimDriver->loopOnce();
//    }
//    else
//    {

//    }
//    return result;
//#endif
//}

//void RosApi::update_laser_driver_config(LaserDriverConfig configIn)
//{
//    #ifdef D_USE_SICK_TIM_DRIVER
//    if(false==isUseSickTimDriver)
//    {
//        return ;
//    }
////    sick_tim::SickTimConfig new_config;
////    new_config.min_ang=configIn.min_ang;
////    new_config.max_ang=configIn.max_ang;
////    new_config.intensity=configIn.intensity;
////    new_config.skip=configIn.skip;
////    new_config.frame_id=configIn.frame_id;
////    new_config.time_offset=configIn.time_offset;
////    new_config.auto_reboot=configIn.auto_reboot;
//    sickTimDriver->update_laser_driver_config(configIn.frame_id);
//    #endif
//}

int RosApi::initialAmclAdvance()
{
    #ifdef D_USE_AMCL_ADVANCE
    if(false==isUseAmclAdvance)
    {
        return -1;
    }
    QString tmpconfigFile=D_TCRCONTROLLER_DATA_PATH;
    tmpconfigFile+=D_ROS_CONFIG_FILE;
    tmpconfigFile+="amclConfig.xml";

    QString tmpRootFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString tmpIniFile=tmpRootFilePath+ROBOT_COMMON_NAME+QString::number(1)+"/config/runConfig.ini";

    amclConfig=new AmclConfig(tmpconfigFile,tmpIniFile);
    if(1!=amclConfig->readConfigFile(tmpconfigFile,tmpIniFile))
    {
        addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8159, robotId );//amclError
    }
    amclAdvance=new AmclNode(amclConfig);
    VehiclePosition agvPoseIn;
    amclAdvance->getInitialPose(agvPoseIn.x,agvPoseIn.y,agvPoseIn.rotate);
    reflectPositionMatch->setInitialVehiclePosition(agvPoseIn);
    return 1;
    #endif
}

int RosApi::initialLaserOdometry()
{
    #ifdef D_USE_LASER_ODOMETRY
    if(false==isUseLaserOdometry)
    {
        return -1;
    }
    laserOdometry=new CLaserOdometry2D(skipFrameCount,skipLaserPointCount);
    return 1;
    #endif

}

int RosApi::amclAdvanceLoopOnce()
{
    #ifdef D_USE_AMCL_ADVANCE
    if(false==isUseAmclAdvance)
    {
        return -1;
    }
    int tmpKey=1;
    amclAdvance->requestCountPlus();

    if(amclMaxDogCount<amclAdvance->getLaserRequestCount())
    {
        //amclError AmclNode节点超过amclMaxDogCount都没有收到激光消息。不能作为激光是否丢失的依据，可能某一次处理时间很长。
         addMsg(ENUM_MSG_WARNING, "RosApi", "RosApi", 8165, robotId,amclMaxDogCount,
                                amclAdvance->getLaserRequestCount());
//         return -1;//影响后面报错。
         tmpKey=-1;
    }
    else
    {

    }
    if(false==amclAdvance->isLaserReceiveOkStatus())
    {
        //amclError　AmclNode节点原来的激光超时接收判断，超时10~15s
         addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8152, robotId );//必须要报错，停下来。
//         addMsg(ENUM_MSG_WARNING, "RosApi", "RosApi", 8152, robotId );//for test
//         return -1;
         tmpKey=-2;
    }
    if(false==amclAdvance->isPfOkStatus())
    {
        //报这个错误好像对amcl定位没有影响，注释掉
//         addMsg(ENUM_MSG_ERROR, "RosApi", "RosApi", 8156, robotId );//amclError
//         return -1;
        tmpKey=-3;
    }
    return tmpKey;
    #endif
}


int RosApi::insertPointCloud(PointCloudData_3D &target, const PointCloudData_3D &source)
{
    target.deviceName+=source.deviceName;
    target.point_x.insert(target.point_x.end(), source.point_x.begin(), source.point_x.end());
    target.point_y.insert(target.point_y.end(), source.point_y.begin(), source.point_y.end());
    target.point_z.insert(target.point_z.end(), source.point_z.begin(), source.point_z.end());
    return 1;

}



void RosApi::robotPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& pose)
{
//    rosVehiclePosition.x =pose.pose.pose.position.x;
//    rosVehiclePosition.y =pose.pose.pose.position.y;
//    tf::Quaternion q(pose.pose.pose.orientation.x,pose.pose.pose.orientation.y,
//                     pose.pose.pose.orientation.z,pose.pose.pose.orientation.w);
//    double roll,pitch,yaw;
//    tf::Matrix3x3 m(q);
//    m.getRPY(roll,pitch,yaw);
//    rosVehiclePosition.rotate = yaw;4
//    printf("RosApi::robotPoseCallback: %f %f  \n",pose.pose.pose.position.x,pose.pose.pose.position.y
//           );
}

void RosApi::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotIdIn,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {
    case 8179:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("正在创建地图中！");
        break;
    }
    case 8178:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("后激光雷达没有光强度信息！");
        break;
    }
    case 8177:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("前激光雷达没有光强度信息！");
        break;
    }
    case 8176:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("加载地图失败,图片(%1)不存在！").arg(message);
        break;
    }
    case 8175:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("加载地图失败,文件(%1)不存在！").arg(message);
        break;
    }
    case 8174:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("%1地图名与图片名称不一致，加载地图失败！").arg(message);
        break;
    }
    case 8173:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("地图名位空，加载地图失败！");
        break;
    }
    case 8172:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("agv位置为无穷大，保存到文件失败");
        break;
    }
    case 8171:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("后激光雷达的激光原始数据超时次数(%1)超过出错值(%2)").arg(parameter1).arg(parameter2);
        break;
    }
    case 8170:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("前激光雷达的激光原始数据超时次数(%1)超过出错值(%2)").arg(parameter1).arg(parameter2);
        break;
    }
    case 8169:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("加载地图失败，地图文件不存在,文件")+message;
        break;
    }
    case 8168:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("后激光雷达的激光原始数据超时次数(%1)超过提醒值(%2)").arg(parameter1).arg(parameter2);
        break;
    }
    case 8167:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("tf坐标系错误");
        break;
    }
    case 8166:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("前激光雷达的激光原始数据超时次数(%1)超过提醒值(%2)").arg(parameter1).arg(parameter2);
        break;
    }
    case 8165:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("amcl失败次数(%2)超过提醒值(%1)").arg(parameter1).arg(parameter2);
        break;
    }
    case 8164:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("创建地图结束");
        break;
    }
    case 8163:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("开始创建地图...");
        break;
    }
    case 8162:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("激光雷达内部驱动获取数据帧失败，雷达序号%1").arg(parameter1);
        break;
    }
    case 8161:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("激光雷达内部驱动初始化失败，雷达序号%1").arg(parameter1);
        break;
    }
    case 8160:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("amcl看门狗计数超过%1，amcl没有正常计算成功").arg(parameter1);
        break;
    }
    case 8159:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("amclConfig.xml配置文件读取失败");
        break;
    }
    case 8158:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("反光条位置识别失败");
        break;
    }
    case 8157:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("rosCommonConfig.xml读取失败");
        break;
    }
    case 8156:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("amcl pf failed　更新失败");
        break;
    }
    case 8155:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("3dCamera.xml 3D相机配置文件读取失败");
        break;
    }
    case 8154:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("laserScan.xml雷达配置文件读取失败");
        break;
    }
    case 8153:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("sick激光雷达驱动初始化失败！");
        break;
    }
    case 8152:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("amcl超过设定时间，没有收到激光数据！");
        break;
    }
    case 8151:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("局部地图区域选择不存在！");
        break;
    }
    case 8150:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("全局地图区域选择不存在！");
        break;
    }




    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}




